Rover: move update_sensor_status_flags into GCS subclasses

APMrover2/APMrover2.cpp

@@ -299,7 +299,7 @@ void Rover::one_second_loop(void)
     // MAV_SYS_STATUS_* values from mavlink. If a bit is set then it
     // indicates that the sensor or subsystem is present but not
     // functioning correctly
-    update_sensor_status_flags();
+    gcs().update_sensor_status_flags();
 
     // need to set "likely flying" when armed to allow for compass
     // learning to run

APMrover2/GCS_Mavlink.cpp

@@ -75,17 +75,6 @@ MAV_STATE GCS_MAVLINK_Rover::system_status() const
     return MAV_STATE_ACTIVE;
 }
 
-void GCS_Rover::get_sensor_status_flags(uint32_t &present,
-                                                uint32_t &enabled,
-                                                uint32_t &health)
-{
-    rover.update_sensor_status_flags();
-
-    present = rover.control_sensors_present;
-    enabled = rover.control_sensors_enabled;
-    health = rover.control_sensors_health;
-}
-
 void GCS_MAVLINK_Rover::send_nav_controller_output() const
 {
     if (!rover.control_mode->is_autopilot_mode()) {

APMrover2/GCS_Rover.h

@@ -17,7 +17,7 @@ public:
     // return GCS link at offset ofs
     const GCS_MAVLINK_Rover &chan(const uint8_t ofs) const override { return _chan[ofs]; };
 
-    void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
+    void update_sensor_status_flags(void) override;
 
 private:
 

APMrover2/Rover.h

@@ -302,10 +302,6 @@ private:
     AP_DEVO_Telem devo_telemetry;
 #endif
 
-    uint32_t control_sensors_present;
-    uint32_t control_sensors_enabled;
-    uint32_t control_sensors_health;
-
     // 3D Location vectors
     // Location structure defined in AP_Common
     // The home location used for RTL.  The location is set when we first get stable GPS lock
@@ -493,7 +489,6 @@ private:
     void read_rangefinders(void);
     void init_proximity();
     void read_airspeed();
-    void update_sensor_status_flags(void);
 
     // Steering.cpp
     bool use_pivot_steering_at_next_WP(float yaw_error_cd);

APMrover2/sensors.cpp

@@ -1,6 +1,7 @@
 #include "Rover.h"
 
 #include <AP_RangeFinder/RangeFinder_Backend.h>
+#include <AP_VisualOdom/AP_VisualOdom.h>
 
 // initialise compass
 void Rover::init_compass()
@@ -240,25 +241,29 @@ void Rover::rpm_update(void)
 // uses the MAV_SYS_STATUS_* values from mavlink. If a bit is set
 // then it indicates that the sensor or subsystem is present but
 // not functioning correctly.
-void Rover::update_sensor_status_flags(void)
+void GCS_Rover::update_sensor_status_flags(void)
 {
     // default sensors present
     control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
 
     // first what sensors/controllers we have
-    if (g.compass_enabled) {
+    if (rover.g.compass_enabled) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG;  // compass present
     }
+    const AP_GPS &gps = AP::gps();
     if (gps.status() > AP_GPS::NO_GPS) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
     }
-    if (g2.visual_odom.enabled()) {
+    const AP_VisualOdom *visual_odom = AP::visualodom();
+    if (visual_odom && visual_odom->enabled()) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION;
     }
-    if (rover.logger.logging_present()) {  // primary logging only (usually File)
+    const AP_Logger &logger = AP::logger();
+    if (logger.logging_present()) {  // primary logging only (usually File)
         control_sensors_present |= MAV_SYS_STATUS_LOGGING;
     }
-    if (rover.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) {
+    const AP_Proximity *proximity = AP_Proximity::get_singleton();
+    if (proximity && proximity->get_status() > AP_Proximity::Proximity_NotConnected) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
     }
 
@@ -270,16 +275,16 @@ void Rover::update_sensor_status_flags(void)
                                                          ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS &
                                                          ~MAV_SYS_STATUS_LOGGING &
                                                          ~MAV_SYS_STATUS_SENSOR_BATTERY);
-    if (control_mode->attitude_stabilized()) {
+    if (rover.control_mode->attitude_stabilized()) {
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // 3D angular rate control
     }
-    if (control_mode->is_autopilot_mode()) {
+    if (rover.control_mode->is_autopilot_mode()) {
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control
     }
 
-    if (rover.logger.logging_enabled()) {
+    if (logger.logging_enabled()) {
         control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
     }
 
@@ -288,21 +293,26 @@ void Rover::update_sensor_status_flags(void)
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS;
     }
 
+    const AP_BattMonitor &battery = AP::battery();
     if (battery.num_instances() > 0) {
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
     }
 
+    AP_AHRS &ahrs = AP::ahrs();
+    const Compass &compass = AP::compass();
+
     // default to all healthy except compass and gps which we set individually
     control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS);
-    if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
+    if (rover.g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
     if (gps.is_healthy()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
-    if (g2.visual_odom.enabled() && !g2.visual_odom.healthy()) {
+    if (visual_odom && visual_odom->enabled() && !visual_odom->healthy()) {
         control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION;
     }
+    const AP_InertialSensor &ins = AP::ins();
     if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) {
         control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO;
     }
@@ -315,18 +325,19 @@ void Rover::update_sensor_status_flags(void)
         control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
     }
 
-    if (rangefinder.num_sensors() > 0) {
+    const RangeFinder *rangefinder = RangeFinder::get_singleton();
+    if (rangefinder && rangefinder->num_sensors() > 0) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
-        AP_RangeFinder_Backend *s = rangefinder.get_backend(0);
+        AP_RangeFinder_Backend *s = rangefinder->get_backend(0);
         if (s != nullptr && s->has_data()) {
             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
     }
-    if (rover.g2.proximity.get_status() == AP_Proximity::Proximity_NoData) {
+    if (proximity && proximity->get_status() == AP_Proximity::Proximity_NoData) {
         control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION;
     }
-    if (rover.logger.logging_failed()) {
+    if (logger.logging_failed()) {
         control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
     }
 
@@ -334,13 +345,13 @@ void Rover::update_sensor_status_flags(void)
         control_sensors_enabled &= ~MAV_SYS_STATUS_SENSOR_BATTERY;
     }
 
-    if (!initialised || ins.calibrating()) {
+    if (!rover.initialised || ins.calibrating()) {
         // while initialising the gyros and accels are not enabled
         control_sensors_enabled &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
         control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
     }
 #if FRSKY_TELEM_ENABLED == ENABLED
     // give mask of error flags to Frsky_Telemetry
-    frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);
+    rover.frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);
 #endif
 }