Added public set armed and set flight mode interfaces

mavlink · Feb 18, 2025 · e827c91 · e827c91
1 parent b4f133d
commit e827c91
Show file tree

Hide file tree

Showing 10 changed files with 4,434 additions and 1,731 deletions.
diff --git a/proto b/proto
diff --git a/src/mavsdk/plugins/action_server/action_server.cpp b/src/mavsdk/plugins/action_server/action_server.cpp
@@ -117,6 +117,16 @@ ActionServer::AllowableFlightModes ActionServer::get_allowable_flight_modes() co
     return _impl->get_allowable_flight_modes();
 }
 
+ActionServer::Result ActionServer::set_armed_state(bool is_armed) const
+{
+    return _impl->set_armed_state(is_armed);
+}
+
+ActionServer::Result ActionServer::set_flight_mode(FlightMode flight_mode) const
+{
+    return _impl->set_flight_mode(flight_mode);
+}
+
 bool operator==(
     const ActionServer::AllowableFlightModes& lhs, const ActionServer::AllowableFlightModes& rhs)
 {

diff --git a/src/mavsdk/plugins/action_server/action_server_impl.cpp b/src/mavsdk/plugins/action_server/action_server_impl.cpp
@@ -2,14 +2,15 @@
 #include "unused.h"
 #include "flight_mode.h"
 #include "callback_list.tpp"
+#include "px4_custom_mode.h"
 
 namespace mavsdk {
 
 template class CallbackList<ActionServer::Result, ActionServer::ArmDisarm>;
 template class CallbackList<ActionServer::Result, ActionServer::FlightMode>;
 template class CallbackList<ActionServer::Result, bool>;
 
-ActionServer::FlightMode telemetry_flight_mode_from_flight_mode(FlightMode flight_mode)
+ActionServer::FlightMode ActionServerImpl::telemetry_flight_mode_from_flight_mode(FlightMode flight_mode)
 {
     switch (flight_mode) {
         case FlightMode::Ready:
@@ -43,6 +44,69 @@ ActionServer::FlightMode telemetry_flight_mode_from_flight_mode(FlightMode fligh
     }
 }
 
+uint32_t ActionServerImpl::to_px4_mode_from_flight_mode(ActionServer::FlightMode flight_mode) {
+    px4::px4_custom_mode px4_mode;
+    switch (flight_mode) {
+        case ActionServer::FlightMode::Ready:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_READY;
+            break;
+        case ActionServer::FlightMode::Takeoff:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_TAKEOFF;
+            break;
+        case ActionServer::FlightMode::Hold:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LOITER;
+            break;
+        case ActionServer::FlightMode::Mission:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_MISSION;
+            break;
+        case ActionServer::FlightMode::ReturnToLaunch:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_RTL;
+            break;
+        case ActionServer::FlightMode::Land:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_LAND;
+            break;
+        case ActionServer::FlightMode::Offboard:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_OFFBOARD;
+            px4_mode.sub_mode = 0;
+            break;
+        case ActionServer::FlightMode::FollowMe:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_AUTO;
+            px4_mode.sub_mode = px4::PX4_CUSTOM_SUB_MODE_AUTO_FOLLOW_TARGET;
+            break;
+        case ActionServer::FlightMode::Manual:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_MANUAL;
+            px4_mode.sub_mode = 0;
+            break;
+        case ActionServer::FlightMode::Posctl:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_POSCTL;
+            px4_mode.sub_mode = 0;
+            break;
+        case ActionServer::FlightMode::Altctl:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ALTCTL;
+            px4_mode.sub_mode = 0;
+            break;
+        case ActionServer::FlightMode::Acro:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_ACRO;
+            px4_mode.sub_mode = 0;
+            break;
+        case ActionServer::FlightMode::Stabilized:
+            px4_mode.main_mode = px4::PX4_CUSTOM_MAIN_MODE_STABILIZED;
+            px4_mode.sub_mode = 0;
+            break;
+        default:
+            px4_mode.main_mode = 0; // unknown
+            px4_mode.sub_mode = 0;
+            break;
+    }
+    return px4_mode.data;
+}
+
 ActionServerImpl::ActionServerImpl(std::shared_ptr<ServerComponent> server_component) :
     ServerPluginImplBase(server_component)
 {
@@ -138,7 +202,7 @@ void ActionServerImpl::init()
 
             if (is_custom) {
                 // PX4 uses custom modes
-                px4_custom_mode px4_mode{};
+                px4::px4_custom_mode px4_mode{};
                 px4_mode.main_mode = custom_mode;
                 px4_mode.sub_mode = sub_custom_mode;
                 auto system_flight_mode = to_flight_mode_from_px4_mode(px4_mode.data);
@@ -175,7 +239,7 @@ void ActionServerImpl::init()
             }
 
             // PX4...
-            px4_custom_mode px4_mode{};
+            px4::px4_custom_mode px4_mode{};
             px4_mode.data = get_custom_mode();
 
             if (allow_mode) {
@@ -333,6 +397,42 @@ ActionServer::AllowableFlightModes ActionServerImpl::get_allowable_flight_modes(
     return _allowed_flight_modes;
 }
 
+ActionServer::Result ActionServerImpl::set_armed_state(bool armed) {
+    std::lock_guard<std::mutex> lock(_callback_mutex);
+    ActionServer::ArmDisarm arm_disarm{
+        .arm = armed,
+        .force = true
+    };
+
+    set_server_armed(armed);
+    _arm_disarm_callbacks.queue(ActionServer::Result::Success, arm_disarm, [this](const auto& func) {
+        _server_component_impl->call_user_callback(func);
+    });
+    return ActionServer::Result::Success;
+}
+
+ActionServer::Result ActionServerImpl::set_flight_mode(ActionServer::FlightMode flight_mode) {
+    // note: currently on receipt of MAV_CMD_DO_SET_MODE, we error out if the mode
+    // is *not* PX4 custom. For symmetry we will also convert from FlightMode to
+    // PX4 custom when set directly.
+    std::lock_guard<std::mutex> lock(_callback_mutex);
+
+    px4::px4_custom_mode px4_mode{};
+    px4_mode.data = to_px4_mode_from_flight_mode(flight_mode);
+
+    uint8_t base_mode = get_base_mode();
+    base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED;
+    set_base_mode(base_mode);
+    set_custom_mode(px4_mode.data);
+
+    _flight_mode_change_callbacks.queue(
+        ActionServer::Result::Success,
+        flight_mode,
+        [this](const auto& func) { _server_component_impl->call_user_callback(func); });
+
+    return ActionServer::Result::Success;
+}
+
 void ActionServerImpl::set_base_mode(uint8_t base_mode)
 {
     _server_component_impl->set_base_mode(base_mode);

diff --git a/src/mavsdk/plugins/action_server/action_server_impl.h b/src/mavsdk/plugins/action_server/action_server_impl.h
@@ -51,7 +51,14 @@ class ActionServerImpl : public ServerPluginImplBase {
 
     ActionServer::AllowableFlightModes get_allowable_flight_modes();
 
+    ActionServer::Result set_armed_state(bool armed);
+
+    ActionServer::Result set_flight_mode(ActionServer::FlightMode flight_mode);
+
 private:
+    static ActionServer::FlightMode telemetry_flight_mode_from_flight_mode(FlightMode flight_mode);
+    static uint32_t to_px4_mode_from_flight_mode(ActionServer::FlightMode flight_mode);
+
     void set_base_mode(uint8_t base_mode);
     uint8_t get_base_mode() const;
 
@@ -75,16 +82,6 @@ class ActionServerImpl : public ServerPluginImplBase {
     std::atomic<bool> _force_disarmable = false;
     std::atomic<bool> _allow_takeoff = false;
 
-    union px4_custom_mode {
-        struct {
-            uint16_t reserved;
-            uint8_t main_mode;
-            uint8_t sub_mode;
-        };
-        uint32_t data;
-        float data_float;
-    };
-
     std::mutex _flight_mode_mutex;
     ActionServer::AllowableFlightModes _allowed_flight_modes{};
     std::atomic<ActionServer::FlightMode> _flight_mode{ActionServer::FlightMode::Unknown};

diff --git a/src/mavsdk/plugins/action_server/include/plugins/action_server/action_server.h b/src/mavsdk/plugins/action_server/include/plugins/action_server/action_server.h
@@ -350,6 +350,24 @@ class ActionServer : public ServerPluginBase {
      */
     ActionServer::AllowableFlightModes get_allowable_flight_modes() const;
 
+    /**
+     * @brief Set/override the armed/disarmed state of the vehicle directly, and notify subscribers
+     *
+     * This function is blocking.
+     *
+     * @return Result of request.
+     */
+    Result set_armed_state(bool is_armed) const;
+
+    /**
+     * @brief Set/override the vehicle flight mode directly, and notify subscribers
+     *
+     * This function is blocking.
+     *
+     * @return Result of request.
+     */
+    Result set_flight_mode(FlightMode flight_mode) const;
+
     /**
      * @brief Copy constructor.
      */

diff --git a/src/mavsdk_server/src/generated/action_server/action_server.grpc.pb.cc b/src/mavsdk_server/src/generated/action_server/action_server.grpc.pb.cc