Plane: move update_sensor_status_flags into GCS subclasses

ArduPlane/ArduPlane.cpp

@@ -305,7 +305,7 @@ void Plane::one_second_loop()
     // 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();
 }
 
 void Plane::compass_save()

ArduPlane/GCS_Mavlink.cpp

@@ -153,17 +153,6 @@ void Plane::send_fence_status(mavlink_channel_t chan)
 #endif
 
 
-void GCS_Plane::get_sensor_status_flags(uint32_t &present,
-                                                uint32_t &enabled,
-                                                uint32_t &health)
-{
-    plane.update_sensor_status_flags();
-
-    present = plane.control_sensors_present;
-    enabled = plane.control_sensors_enabled;
-    health = plane.control_sensors_health;
-}
-
 void GCS_MAVLINK_Plane::send_nav_controller_output() const
 {
     if (plane.control_mode == MANUAL) {

ArduPlane/GCS_Plane.h

@@ -22,7 +22,9 @@ public:
 
     void send_airspeed_calibration(const Vector3f &vg);
 
-    void get_sensor_status_flags(uint32_t &present, uint32_t &enabled, uint32_t &health);
+protected:
+
+    void update_sensor_status_flags(void) override;
 
 private:
 

ArduPlane/Plane.h

@@ -417,11 +417,6 @@ private:
     AP_DEVO_Telem devo_telemetry;
 #endif
 
-    // Variables for extended status MAVLink messages
-    uint32_t control_sensors_present;
-    uint32_t control_sensors_enabled;
-    uint32_t control_sensors_health;
- 
     // Airspeed Sensors
     AP_Airspeed airspeed;
 
@@ -798,7 +793,6 @@ private:
     void adjust_nav_pitch_throttle(void);
     void update_load_factor(void);
     void send_fence_status(mavlink_channel_t chan);
-    void update_sensor_status_flags(void);
     void send_servo_out(mavlink_channel_t chan);
     void send_wind(mavlink_channel_t chan);
     void send_pid_info(const mavlink_channel_t chan, const AP_Logger::PID_Info *pid_info, const uint8_t axis, const float achieved);

ArduPlane/sensors.cpp

@@ -1,5 +1,6 @@
 #include "Plane.h"
 #include <AP_RSSI/AP_RSSI.h>
+#include <AP_OpticalFlow/AP_OpticalFlow.h>
 
 /*
   read the rangefinder and update height estimate
@@ -96,58 +97,63 @@ void Plane::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 Plane::update_sensor_status_flags(void)
+void GCS_Plane::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 (plane.g.compass_enabled) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
     }
 
-    if (airspeed.enabled()) {
+    const AP_Airspeed *airspeed = AP_Airspeed::get_singleton();
+    if (airspeed && airspeed->enabled()) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
     }
+    const AP_GPS &gps = AP::gps();
     if (gps.status() > AP_GPS::NO_GPS) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS;
     }
 #if OPTFLOW == ENABLED
-    if (optflow.enabled()) {
+    const OpticalFlow *optflow = AP::opticalflow();
+    if (optflow && optflow->enabled()) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
     }
 #endif
-    if (geofence_present()) {
+    if (plane.geofence_present()) {
         control_sensors_present |= MAV_SYS_STATUS_GEOFENCE;
     }
 
-    if (have_reverse_thrust()) {
+    if (plane.have_reverse_thrust()) {
         control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR;
     }
-    if (plane.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;
     }
 
     // all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control, geofence, motor, and battery output which we will set individually
     control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL & ~MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION & ~MAV_SYS_STATUS_SENSOR_YAW_POSITION & ~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_GEOFENCE & ~MAV_SYS_STATUS_LOGGING & ~MAV_SYS_STATUS_SENSOR_BATTERY);
 
-    if (airspeed.enabled() && airspeed.use()) {
+    if (airspeed && airspeed->enabled() && airspeed->use()) {
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
     }
 
-    if (geofence_enabled()) {
+    if (plane.geofence_enabled()) {
         control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE;
     }
 
-    if (plane.logger.logging_enabled()) {
+    if (logger.logging_enabled()) {
         control_sensors_enabled |= MAV_SYS_STATUS_LOGGING;
     }
 
+    const AP_BattMonitor &battery = AP::battery();
     if (battery.num_instances() > 0) {
         control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_BATTERY;
     }
 
-    switch (control_mode) {
+    switch (plane.control_mode) {
     case MANUAL:
         break;
 
@@ -174,7 +180,7 @@ void Plane::update_sensor_status_flags(void)
         break;
 
     case TRAINING:
-        if (!training_manual_roll || !training_manual_pitch) {
+        if (!plane.training_manual_roll || !plane.training_manual_pitch) {
             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control
             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION; // attitude stabilisation        
         }
@@ -210,26 +216,30 @@ void Plane::update_sensor_status_flags(void)
                                                          MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE);
     control_sensors_health |= MAV_SYS_STATUS_GEOFENCE;
 
+    AP_AHRS &ahrs = AP::ahrs();
     if (ahrs.initialised() && !ahrs.healthy()) {
         // AHRS subsystem is unhealthy
         control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
     }
+    const AP_InertialSensor &ins = AP::ins();
     if (ahrs.have_inertial_nav() && !ins.accel_calibrated_ok_all()) {
         // trying to use EKF without properly calibrated accelerometers
         control_sensors_health &= ~MAV_SYS_STATUS_AHRS;
     }
 
+    const AP_Baro &barometer = AP::baro();
     if (barometer.all_healthy()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE;
     }
-    if (g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
+    const Compass &compass = AP::compass();
+    if (plane.g.compass_enabled && compass.healthy() && ahrs.use_compass()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
     }
     if (gps.status() >= AP_GPS::GPS_OK_FIX_3D && gps.is_healthy()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
     }
 #if OPTFLOW == ENABLED
-    if (optflow.healthy()) {
+    if (optflow && optflow->healthy()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW;
     }
 #endif
@@ -239,27 +249,27 @@ void Plane::update_sensor_status_flags(void)
     if (!ins.get_accel_health_all()) {
         control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL;
     }
-    if (airspeed.all_healthy()) {
+    if (airspeed && airspeed->all_healthy()) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
     }
 #if GEOFENCE_ENABLED
-    if (geofence_breached()) {
+    if (plane.geofence_breached()) {
         control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE;
     }
 #endif
 
-    if (plane.logger.logging_failed()) {
+    if (logger.logging_failed()) {
         control_sensors_health &= ~MAV_SYS_STATUS_LOGGING;
     }
 
-    if (millis() - failsafe.last_valid_rc_ms < 200) {
+    if (millis() - plane.failsafe.last_valid_rc_ms < 200) {
         control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
     } else {
         control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
     }
 
 #if AP_TERRAIN_AVAILABLE
-    switch (terrain.status()) {
+    switch (plane.terrain.status()) {
     case AP_Terrain::TerrainStatusDisabled:
         break;
     case AP_Terrain::TerrainStatusUnhealthy:
@@ -274,17 +284,18 @@ void Plane::update_sensor_status_flags(void)
     }
 #endif
 
-    if (rangefinder.has_orientation(ROTATION_PITCH_270)) {
+    const RangeFinder *rangefinder = RangeFinder::get_singleton();
+    if (rangefinder && rangefinder->has_orientation(ROTATION_PITCH_270)) {
         control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
-        if (g.rangefinder_landing) {
+        if (plane.g.rangefinder_landing) {
             control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;
         }
-        if (rangefinder.has_data_orient(ROTATION_PITCH_270)) {
+        if (rangefinder->has_data_orient(ROTATION_PITCH_270)) {
             control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION;            
         }
     }
 
-    if (have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) {
+    if (plane.have_reverse_thrust() && SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) < 0) {
         control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR;
         control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR;
     }
@@ -301,6 +312,6 @@ void Plane::update_sensor_status_flags(void)
 
 #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);
+    plane.frsky_telemetry.update_sensor_status_flags(~control_sensors_health & control_sensors_enabled & control_sensors_present);
 #endif
 }