Added function to query SYS_STATUS present/enabled/health flags

src/mavsdk/plugins/telemetry/include/plugins/telemetry/telemetry.h

@@ -431,6 +431,18 @@ 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 +1440,13 @@ class Telemetry : public PluginBase {
      */
     Health health() const;
 
+    /**
+     * @brief Poll for 'sys_status_sensors' (blocking).
+     *
+     * @return One SysStatusSensors update.
+     */
+    SysStatusSensors sys_status_sensors() const;
+
     /**
      * @brief Callback type for subscribe_rc_status.
      */

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

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<std::mutex> 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<std::mutex> lock(_sys_status_sensors_mutex);
+    return _sys_status_sensors;
+}
+
 Telemetry::RcStatus TelemetryImpl::rc_status() const
 {
     std::lock_guard<std::mutex> lock(_rc_status_mutex);

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