From 5a2c420f1efe188a86bc6ec84945fceea1c5ad8e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 21 Aug 2024 14:26:11 -0800 Subject: [PATCH 1/3] added function to query SYS_STATUS present/enabled/health --- .../include/plugins/telemetry/telemetry.h | 11 +++++++++++ src/mavsdk/plugins/telemetry/telemetry.cpp | 5 +++++ src/mavsdk/plugins/telemetry/telemetry_impl.cpp | 15 +++++++++++++++ src/mavsdk/plugins/telemetry/telemetry_impl.h | 5 +++++ 4 files changed, 36 insertions(+) diff --git a/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h b/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h index 07b1addab..384ecd62f 100644 --- a/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -431,6 +431,15 @@ class Telemetry : public PluginBase { */ friend std::ostream& operator<<(std::ostream& str, Telemetry::Health const& health); + /** + * @brief SysStatusSensors type. + */ + struct SysStatusSensors { + uint32_t present{0}; /**< Bitmap showing which onboard controllers and sensors are present */ + uint32_t enabled{0}; /**< Bitmap showing which onboard controllers and sensors are enabled */ + uint32_t health{0}; /**< Bitmap showing which onboard controllers and sensors have an error */ + }; + /** * @brief Remote control status type. */ @@ -1428,6 +1437,8 @@ class Telemetry : public PluginBase { */ Health health() const; + SysStatusSensors sys_status_sensors() const; + /** * @brief Callback type for subscribe_rc_status. */ diff --git a/src/mavsdk/plugins/telemetry/telemetry.cpp b/src/mavsdk/plugins/telemetry/telemetry.cpp index f8b04f79f..25a5b6103 100644 --- a/src/mavsdk/plugins/telemetry/telemetry.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry.cpp @@ -278,6 +278,11 @@ Telemetry::Health Telemetry::health() const return _impl->health(); } +Telemetry::SysStatusSensors Telemetry::sys_status_sensors() const +{ + return _impl->sys_status_sensors(); +} + Telemetry::RcStatusHandle Telemetry::subscribe_rc_status(const RcStatusCallback& callback) { return _impl->subscribe_rc_status(callback); diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp index 570e223aa..e6561790a 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.cpp +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.cpp @@ -1051,6 +1051,7 @@ void TelemetryImpl::process_sys_status(const mavlink_message_t& message) set_health_local_position(local_position_ok); set_health_global_position(global_position_ok); + set_sys_status_sensors(sys_status); set_rc_status({rc_ok}, std::nullopt); @@ -1064,6 +1065,14 @@ void TelemetryImpl::process_sys_status(const mavlink_message_t& message) health_all_ok(), [this](const auto& func) { _system_impl->call_user_callback(func); }); } +void TelemetryImpl::set_sys_status_sensors(const mavlink_sys_status_t& sys_status) +{ + std::lock_guard lock(_sys_status_sensors_mutex); + _sys_status_sensors.present = sys_status.onboard_control_sensors_present; + _sys_status_sensors.enabled = sys_status.onboard_control_sensors_enabled; + _sys_status_sensors.health = sys_status.onboard_control_sensors_health; +} + bool TelemetryImpl::sys_status_present_enabled_health( const mavlink_sys_status_t& sys_status, MAV_SYS_STATUS_SENSOR flag) { @@ -1896,6 +1905,12 @@ bool TelemetryImpl::health_all_ok() const } } +Telemetry::SysStatusSensors TelemetryImpl::sys_status_sensors() const +{ + std::lock_guard lock(_sys_status_sensors_mutex); + return _sys_status_sensors; +} + Telemetry::RcStatus TelemetryImpl::rc_status() const { std::lock_guard lock(_rc_status_mutex); diff --git a/src/mavsdk/plugins/telemetry/telemetry_impl.h b/src/mavsdk/plugins/telemetry/telemetry_impl.h index b1b3a0091..d6c0b4156 100644 --- a/src/mavsdk/plugins/telemetry/telemetry_impl.h +++ b/src/mavsdk/plugins/telemetry/telemetry_impl.h @@ -102,6 +102,7 @@ class TelemetryImpl : public PluginImplBase { Telemetry::FlightMode flight_mode() const; Telemetry::Health health() const; bool health_all_ok() const; + Telemetry::SysStatusSensors sys_status_sensors() const; Telemetry::RcStatus rc_status() const; Telemetry::ActuatorControlTarget actuator_control_target() const; Telemetry::ActuatorOutputStatus actuator_output_status() const; @@ -234,6 +235,7 @@ class TelemetryImpl : public PluginImplBase { void set_scaled_pressure(Telemetry::ScaledPressure& scaled_pressure); void set_heading(Telemetry::Heading heading); void set_altitude(Telemetry::Altitude altitude); + void set_sys_status_sensors(const mavlink_sys_status_t& sys_status); void process_position_velocity_ned(const mavlink_message_t& message); void process_global_position_int(const mavlink_message_t& message); @@ -340,6 +342,9 @@ class TelemetryImpl : public PluginImplBase { mutable std::mutex _health_mutex{}; Telemetry::Health _health{}; + mutable std::mutex _sys_status_sensors_mutex{}; + Telemetry::SysStatusSensors _sys_status_sensors{}; + mutable std::mutex _vtol_state_mutex{}; Telemetry::VtolState _vtol_state{Telemetry::VtolState::Undefined}; From 8402b878cc44d976a87a608e8c2b7dfb8109d38e Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Wed, 21 Aug 2024 14:58:49 -0800 Subject: [PATCH 2/3] fix style --- .../telemetry/include/plugins/telemetry/telemetry.h | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h b/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h index 384ecd62f..5c2b7716d 100644 --- a/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -435,9 +435,12 @@ class Telemetry : public PluginBase { * @brief SysStatusSensors type. */ struct SysStatusSensors { - uint32_t present{0}; /**< Bitmap showing which onboard controllers and sensors are present */ - uint32_t enabled{0}; /**< Bitmap showing which onboard controllers and sensors are enabled */ - uint32_t health{0}; /**< Bitmap showing which onboard controllers and sensors have an error */ + uint32_t present{ + 0}; /**< Bitmap showing which onboard controllers and sensors are present */ + uint32_t enabled{ + 0}; /**< Bitmap showing which onboard controllers and sensors are enabled */ + uint32_t health{ + 0}; /**< Bitmap showing which onboard controllers and sensors have an error */ }; /** From 2a18f6e9951ea505d0a9da7b8eda5862f6a1728b Mon Sep 17 00:00:00 2001 From: Jacob Dahl Date: Thu, 22 Aug 2024 23:36:29 -0800 Subject: [PATCH 3/3] fix style --- .../plugins/telemetry/include/plugins/telemetry/telemetry.h | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h b/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h index 5c2b7716d..295d8b4f4 100644 --- a/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h +++ b/src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h @@ -1440,6 +1440,11 @@ class Telemetry : public PluginBase { */ Health health() const; + /** + * @brief Poll for 'sys_status_sensors' (blocking). + * + * @return One SysStatusSensors update. + */ SysStatusSensors sys_status_sensors() const; /**