ActionServer: allow direct setting of arm/disarm state and vehicle flight mode

docs/en/cpp/api_reference/classmavsdk_1_1_action_server.md

@@ -64,6 +64,8 @@ void | [unsubscribe_terminate](#classmavsdk_1_1_action_server_1a3e236694f1f0beae
 [Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_disarmable](#classmavsdk_1_1_action_server_1afae1336100d7a91a4f4521cee56a1ecb) (bool disarmable, bool force_disarmable)const | Can the vehicle disarm when requested.
 [Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_allowable_flight_modes](#classmavsdk_1_1_action_server_1a3041d1b923a3b01021433ad43ab93b3a) ([AllowableFlightModes](structmavsdk_1_1_action_server_1_1_allowable_flight_modes.md) flight_modes)const | Set which modes the vehicle can transition to (Manual always allowed)
 [ActionServer::AllowableFlightModes](structmavsdk_1_1_action_server_1_1_allowable_flight_modes.md) | [get_allowable_flight_modes](#classmavsdk_1_1_action_server_1a0960a6ec243823729a418a3c68feaf2a) () const | Get which modes the vehicle can transition to (Manual always allowed)
+[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_armed_state](#classmavsdk_1_1_action_server_1a8830660884933029f104c49c31b7af24) (bool is_armed)const | Set/override the armed/disarmed state of the vehicle directly, and notify subscribers.
+[Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) | [set_flight_mode](#classmavsdk_1_1_action_server_1ac5ba6d26aef83881826361aa20a9bd65) ([FlightMode](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1aee12027f5d9380f2c13fa7813c6ae1d8) flight_mode)const | Set/override the flight mode of the vehicle directly, and notify subscribers.
 const [ActionServer](classmavsdk_1_1_action_server.md) & | [operator=](#classmavsdk_1_1_action_server_1aa80e34dd72d2e31005085c78892bab8c) (const [ActionServer](classmavsdk_1_1_action_server.md) &)=delete | Equality operator (object is not copyable).
 
 
@@ -613,6 +615,42 @@ This function is blocking.
 
  [ActionServer::AllowableFlightModes](structmavsdk_1_1_action_server_1_1_allowable_flight_modes.md) - Result of request.
 
+### set_armed_state() {#classmavsdk_1_1_action_server_1a8830660884933029f104c49c31b7af24}
+```cpp
+Result mavsdk::ActionServer::set_armed_state(bool is_armed) const
+```
+
+
+Set/override the armed/disarmed state of the vehicle directly, and notify subscribers.
+
+This function is blocking.
+
+**Parameters**
+
+* bool **is_armed** - 
+
+**Returns**
+
+ [Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) - Result of request.
+
+### set_flight_mode() {#classmavsdk_1_1_action_server_1ac5ba6d26aef83881826361aa20a9bd65}
+```cpp
+Result mavsdk::ActionServer::set_flight_mode(FlightMode flight_mode) const
+```
+
+
+Set/override the flight mode of the vehicle directly, and notify subscribers.
+
+This function is blocking.
+
+**Parameters**
+
+* [FlightMode](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1aee12027f5d9380f2c13fa7813c6ae1d8) **flight_mode** - 
+
+**Returns**
+
+ [Result](classmavsdk_1_1_action_server.md#classmavsdk_1_1_action_server_1a4a8eb4fe9d098a5b7891232fc5bf32f8) - Result of request.
+
 ### operator=() {#classmavsdk_1_1_action_server_1aa80e34dd72d2e31005085c78892bab8c}
 ```cpp
 const ActionServer & mavsdk::ActionServer::operator=(const ActionServer &)=delete

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)
 {

src/mavsdk/plugins/action_server/action_server_impl.cpp

@@ -2,14 +2,16 @@
 #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 +45,70 @@ 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 +204,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 +241,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 +399,44 @@ 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_disarm.arm = armed;
+    arm_disarm.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);

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};

src/mavsdk/plugins/action_server/include/plugins/action_server/action_server.h

@@ -350,6 +350,28 @@ 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 flight mode of the vehicle directly, and notify subscribers
+     *
+     * This function is blocking.
+     *
+
+     * @return Result of request.
+
+     */
+    Result set_flight_mode(FlightMode flight_mode) const;
+
     /**
      * @brief Copy constructor.
      */