Add set function to system status

src/mavsdk/core/include/mavsdk/mavsdk.h

@@ -272,6 +272,15 @@ class Mavsdk {
      */
     void set_timeout_s(double timeout_s);
 
+    /**
+     * @brief Set system status of this MAVLink entity.
+     *
+     * The default system status is MAV_STATE_UNINIT.
+     *
+     * @param system_status system status.
+     */
+    void set_system_status(uint8_t system_status);
+
     /**
      * @brief Callback type discover and timeout notifications.
      */

src/mavsdk/core/mavsdk.cpp

@@ -69,6 +69,11 @@ void Mavsdk::set_timeout_s(double timeout_s)
     _impl->set_timeout_s(timeout_s);
 }
 
+void Mavsdk::set_system_status(uint8_t system_status)
+{
+    _impl->set_system_status(system_status);
+}
+
 void Mavsdk::subscribe_on_new_system(const NewSystemCallback& callback)
 {
     _impl->subscribe_on_new_system(callback);

src/mavsdk/core/mavsdk_impl.cpp

@@ -623,7 +623,7 @@ void MavsdkImpl::send_heartbeat()
                                                            MAV_AUTOPILOT_INVALID,
         get_own_component_id() == MAV_COMP_ID_AUTOPILOT1 ? _base_mode.load() : 0,
         get_own_component_id() == MAV_COMP_ID_AUTOPILOT1 ? _custom_mode.load() : 0,
-        0);
+        get_system_status());
     send_message(message);
 }
 
@@ -683,4 +683,14 @@ uint32_t MavsdkImpl::get_custom_mode() const
     return _custom_mode;
 }
 
+void MavsdkImpl::set_system_status(uint8_t system_status)
+{
+    _system_status = system_status;
+}
+
+uint8_t MavsdkImpl::get_system_status()
+{
+    return _system_status;
+}
+
 } // namespace mavsdk

src/mavsdk/core/mavsdk_impl.h

@@ -91,6 +91,8 @@ class MavsdkImpl {
     uint8_t get_base_mode() const;
     void set_custom_mode(uint32_t custom_mode);
     uint32_t get_custom_mode() const;
+    void set_system_status(uint8_t system_status);
+    uint8_t get_system_status();
 
 private:
     void add_connection(const std::shared_ptr<Connection>&);
@@ -150,6 +152,7 @@ class MavsdkImpl {
 
     std::atomic<uint8_t> _base_mode = 0;
     std::atomic<uint32_t> _custom_mode = 0;
+    std::atomic<uint8_t> _system_status = 0;
 };
 
 } // namespace mavsdk