Tracker: move update_sensor_status_flags into GCS subclasses

AntennaTracker/GCS_Mavlink.cpp

@@ -71,17 +71,6 @@ MAV_STATE GCS_MAVLINK_Tracker::system_status() const
     return MAV_STATE_ACTIVE;
 }
 
-void GCS_Tracker::get_sensor_status_flags(uint32_t &present,
-                                                  uint32_t &enabled,
-                                                  uint32_t &health)
-{
-    tracker.update_sensor_status_flags();
-
-    present = tracker.control_sensors_present;
-    enabled = tracker.control_sensors_enabled;
-    health = tracker.control_sensors_health;
-}
-
 void GCS_MAVLINK_Tracker::send_nav_controller_output() const
 {
 	float alt_diff = (tracker.g.alt_source == ALT_SOURCE_BARO) ? tracker.nav_status.alt_difference_baro : tracker.nav_status.alt_difference_gps;

AntennaTracker/GCS_Tracker.h

@@ -17,7 +17,7 @@ public:
     GCS_MAVLINK_Tracker &chan(const uint8_t ofs) override { return _chan[ofs]; };
     const GCS_MAVLINK_Tracker &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() override;
 
 private:
 

AntennaTracker/Tracker.h

@@ -139,11 +139,6 @@ private:
     GCS_Tracker _gcs; // avoid using this; use gcs()
     GCS_Tracker &gcs() { return _gcs; }
 
-    // variables for extended status MAVLink messages
-    uint32_t control_sensors_present;
-    uint32_t control_sensors_enabled;
-    uint32_t control_sensors_health;
-
     AP_BoardConfig BoardConfig;
 
 #if HAL_WITH_UAVCAN
@@ -241,7 +236,6 @@ private:
     void accel_cal_update(void);
     void update_GPS(void);
     void handle_battery_failsafe(const char* type_str, const int8_t action);
-    void update_sensor_status_flags();
 
     // servos.cpp
     void init_servos();

AntennaTracker/sensors.cpp

@@ -91,18 +91,20 @@ void Tracker::handle_battery_failsafe(const char* type_str, const int8_t action)
 }
 
 // update sensors and subsystems present, enabled and healthy flags for reporting to GCS
-void Tracker::update_sensor_status_flags()
+void GCS_Tracker::update_sensor_status_flags()
 {
     // default sensors present
     control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT;
 
     // first what sensors/controllers we have
-    if (g.compass_enabled) {
+    if (tracker.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;
     }
+    const AP_Logger &logger = AP::logger();
     if (logger.logging_present()) {  // primary logging only (usually File)
         control_sensors_present |= MAV_SYS_STATUS_LOGGING;
     }
@@ -121,18 +123,23 @@ void Tracker::update_sensor_status_flags()
         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();
+
     // 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()) {
+    const Compass &compass = AP::compass();
+    if (tracker.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;
     }
+    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;
     }