From 8d3932dfb7f1395972577167768dc2f57f5c314a Mon Sep 17 00:00:00 2001 From: Basti Date: Thu, 10 Mar 2022 18:28:26 +0100 Subject: [PATCH 1/2] add setter function for system status --- src/mavsdk/core/include/mavsdk/mavsdk.h | 9 +++++++++ src/mavsdk/core/mavsdk.cpp | 5 +++++ src/mavsdk/core/mavsdk_impl.cpp | 10 ++++++++++ src/mavsdk/core/mavsdk_impl.h | 3 +++ 4 files changed, 27 insertions(+) diff --git a/src/mavsdk/core/include/mavsdk/mavsdk.h b/src/mavsdk/core/include/mavsdk/mavsdk.h index 13416220bd..55639fdec9 100644 --- a/src/mavsdk/core/include/mavsdk/mavsdk.h +++ b/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. */ diff --git a/src/mavsdk/core/mavsdk.cpp b/src/mavsdk/core/mavsdk.cpp index 9e6220101d..fed322c02f 100644 --- a/src/mavsdk/core/mavsdk.cpp +++ b/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); diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index 9546470499..ebd9cb1d4b 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/src/mavsdk/core/mavsdk_impl.cpp @@ -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 diff --git a/src/mavsdk/core/mavsdk_impl.h b/src/mavsdk/core/mavsdk_impl.h index e96c95723c..8592c61cd2 100644 --- a/src/mavsdk/core/mavsdk_impl.h +++ b/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&); @@ -150,6 +152,7 @@ class MavsdkImpl { std::atomic _base_mode = 0; std::atomic _custom_mode = 0; + std::atomic _system_status = 0; }; } // namespace mavsdk From 8d76b543bbb6dde73edc56531642f576d60bb152 Mon Sep 17 00:00:00 2001 From: Basti Date: Thu, 10 Mar 2022 18:28:46 +0100 Subject: [PATCH 2/2] use system status from setter function --- src/mavsdk/core/mavsdk_impl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/mavsdk/core/mavsdk_impl.cpp b/src/mavsdk/core/mavsdk_impl.cpp index ebd9cb1d4b..4a725d46a0 100644 --- a/src/mavsdk/core/mavsdk_impl.cpp +++ b/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); }