core/action_server: pass through shared state

I previously had commented out some code related to the autopilot
version and heartbeat state messages. This should clean this up,
however, it will need some testing.

src/mavsdk/core/server_component_impl.cpp

@@ -192,7 +192,7 @@ void ServerComponentImpl::send_heartbeat()
 
 void ServerComponentImpl::set_system_status(uint8_t system_status)
 {
-    _system_status = system_status;
+    _system_status = static_cast<MAV_STATE>(system_status);
 }
 
 uint8_t ServerComponentImpl::get_system_status() const
@@ -228,14 +228,13 @@ void ServerComponentImpl::call_user_callback_located(
 
 void ServerComponentImpl::add_capabilities(uint64_t add_capabilities)
 {
-    std::unique_lock<std::mutex> lock(_autopilot_version_mutex);
-    _autopilot_version.capabilities |= add_capabilities;
-
-    // We need to resend capabilities...
-    lock.unlock();
-    if (_should_send_autopilot_version) {
-        send_autopilot_version();
+    {
+        std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
+        _autopilot_version.capabilities |= add_capabilities;
     }
+
+    // We need to resend capabilities.
+    send_autopilot_version();
 }
 
 void ServerComponentImpl::set_flight_sw_version(uint32_t flight_sw_version)

src/mavsdk/core/server_component_impl.h

@@ -125,28 +125,24 @@ class ServerComponentImpl {
     void set_vendor_id(uint16_t vendor_id);
     void set_product_id(uint16_t product_id);
     bool set_uid2(std::string uid2);
-    AutopilotVersion get_autopilot_version_data();
+    void send_autopilot_version();
 
     MavlinkMissionTransfer& mission_transfer() { return _mission_transfer; }
     MAVLinkParameters& mavlink_parameters() { return _mavlink_parameters; }
 
     void do_work();
 
 private:
-    void send_autopilot_version();
-
     MavsdkImpl& _mavsdk_impl;
     uint8_t _own_component_id{MAV_COMP_ID_AUTOPILOT1};
 
-    std::atomic<uint8_t> _system_status{0};
+    std::atomic<MAV_STATE> _system_status{MAV_STATE_UNINIT};
     std::atomic<uint8_t> _base_mode{0};
     std::atomic<uint32_t> _custom_mode{0};
 
     std::mutex _autopilot_version_mutex{};
     AutopilotVersion _autopilot_version{};
 
-    std::atomic<bool> _should_send_autopilot_version{false};
-
     OurSender _our_sender;
     MavlinkCommandReceiver _mavlink_command_receiver;
     MavlinkMissionTransfer _mission_transfer;

src/mavsdk/plugins/action_server/action_server_impl.cpp

@@ -51,7 +51,8 @@ ActionServerImpl::~ActionServerImpl()
 
 void ActionServerImpl::init()
 {
-    enable_sending_autopilot_version();
+    _server_component_impl->add_call_every(
+        [this]() { _server_component_impl->send_autopilot_version(); }, 1.0, &_send_version_cookie);
 
     // Arming / Disarm / Kill
     _server_component_impl->register_mavlink_command_handler(
@@ -196,6 +197,7 @@ void ActionServerImpl::init()
 void ActionServerImpl::deinit()
 {
     _server_component_impl->unregister_all_mavlink_command_handlers(this);
+    _server_component_impl->remove_call_every(_send_version_cookie);
 }
 
 void ActionServerImpl::subscribe_arm_disarm(ActionServer::ArmDisarmCallback callback)
@@ -292,27 +294,6 @@ uint32_t ActionServerImpl::get_custom_mode() const
     return _server_component_impl->get_custom_mode();
 }
 
-void ActionServerImpl::enable_sending_autopilot_version()
-{
-    // FIXME: enable this again
-}
-
-// ActionServerImpl::AutopilotVersion ActionServerImpl::get_autopilot_version_data()
-//{
-//    std::lock_guard<std::mutex> lock(_autopilot_version_mutex);
-//    return _autopilot_version;
-//}
-
-// uint8_t ActionServerImpl::get_autopilot_id() const
-//{
-//    for (auto compid : _components)
-//        if (compid == MavlinkCommandSender::DEFAULT_COMPONENT_ID_AUTOPILOT) {
-//            return compid;
-//        }
-//    // FIXME: Not sure what should be returned if autopilot is not found
-//    return uint8_t(0);
-//}
-
 void ActionServerImpl::set_server_armed(bool armed)
 {
     uint8_t base_mode = get_base_mode();
@@ -324,14 +305,4 @@ void ActionServerImpl::set_server_armed(bool armed)
     set_base_mode(base_mode);
 }
 
-bool ActionServerImpl::is_server_armed() const
-{
-    return (get_base_mode() & MAV_MODE_FLAG_SAFETY_ARMED) == MAV_MODE_FLAG_SAFETY_ARMED;
-}
-
-// void ActionServerImpl::add_capabilities(uint64_t add_capabilities)
-//{
-//    _server_component_impl->add_capabilities(add_capabilities);
-//}
-
 } // namespace mavsdk

src/mavsdk/plugins/action_server/action_server_impl.h

@@ -41,25 +41,19 @@ class ActionServerImpl : public ServerPluginImplBase {
     ActionServer::AllowableFlightModes get_allowable_flight_modes();
 
 private:
-    void set_custom_mode(uint32_t custom_mode);
-    uint32_t get_custom_mode() const;
-
     void set_base_mode(uint8_t base_mode);
     uint8_t get_base_mode() const;
 
-    // Used when acting as autopilot!
-    void set_server_armed(bool armed);
-    bool is_server_armed() const;
+    void set_custom_mode(uint32_t custom_mode);
+    uint32_t get_custom_mode() const;
 
-    void send_autopilot_version();
-    void enable_sending_autopilot_version();
+    void set_server_armed(bool armed);
 
+    std::mutex _callback_mutex;
     ActionServer::ArmDisarmCallback _arm_disarm_callback{nullptr};
     ActionServer::FlightModeChangeCallback _flight_mode_change_callback{nullptr};
     ActionServer::TakeoffCallback _takeoff_callback{nullptr};
 
-    std::mutex _callback_mutex;
-
     std::atomic<bool> _armable = false;
     std::atomic<bool> _force_armable = false;
     std::atomic<bool> _disarmable = false;
@@ -78,14 +72,9 @@ class ActionServerImpl : public ServerPluginImplBase {
 
     std::mutex _flight_mode_mutex;
     ActionServer::AllowableFlightModes _allowed_flight_modes{};
-
-    // std::atomic<bool> _should_send_autopilot_version{false};
-
     std::atomic<ActionServer::FlightMode> _flight_mode{ActionServer::FlightMode::Unknown};
 
-    // std::mutex _autopilot_version_mutex{};
-    // AutopilotVersion _autopilot_version{MAV_PROTOCOL_CAPABILITY_COMMAND_INT, 0, 0, 0, 0, 0, 0,
-    // {0}};
+    void* _send_version_cookie{nullptr};
 };
 
 } // namespace mavsdk