diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index 78e7ef20ed..4be4804723 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/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() diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index b5047a36fe..cbac1df086 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/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) { diff --git a/ArduPlane/GCS_Plane.h b/ArduPlane/GCS_Plane.h index 9ea82978ca..36fceb55b6 100644 --- a/ArduPlane/GCS_Plane.h +++ b/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: diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index cae0cfc7fe..11709807dd 100644 --- a/ArduPlane/Plane.h +++ b/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); diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index a42c9f97bd..399f82512a 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -1,5 +1,6 @@ #include "Plane.h" #include +#include /* 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 }