From bae9ce20c1a5a92c2f674c21080df9621af3eceb Mon Sep 17 00:00:00 2001 From: floaledm Date: Thu, 27 Oct 2016 21:09:35 -0500 Subject: [PATCH] Plane: update sensor status error flags independently of sending a sys_status message --- ArduPlane/ArduPlane.cpp | 6 ++ ArduPlane/GCS_Mavlink.cpp | 209 +------------------------------------- ArduPlane/GCS_Mavlink.h | 3 + ArduPlane/Plane.h | 6 ++ ArduPlane/sensors.cpp | 204 +++++++++++++++++++++++++++++++++++++ 5 files changed, 221 insertions(+), 207 deletions(-) diff --git a/ArduPlane/ArduPlane.cpp b/ArduPlane/ArduPlane.cpp index e6dc55a0bb..b227f0d8e8 100644 --- a/ArduPlane/ArduPlane.cpp +++ b/ArduPlane/ArduPlane.cpp @@ -357,6 +357,12 @@ void Plane::one_second_loop() // reset the landing altitude correction auto_state.land_alt_offset = 0; } + + // update error mask of sensors and subsystems. The mask 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 + update_sensor_status_flags(); } void Plane::log_perf_info() diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index f4db60902c..34edf919aa 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -3,9 +3,6 @@ #include "Plane.h" #include "version.h" -// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control -#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | 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_AHRS | MAV_SYS_STATUS_SENSOR_RC_RECEIVER) - void Plane::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; @@ -124,165 +121,6 @@ void Plane::send_fence_status(mavlink_channel_t chan) void Plane::send_extended_status1(mavlink_channel_t chan) { - uint32_t control_sensors_present; - uint32_t control_sensors_enabled; - uint32_t control_sensors_health; - - // default sensors present - control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; - - // first what sensors/controllers we have - if (g.compass_enabled) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present - } - - if (airspeed.enabled()) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } - if (gps.status() > AP_GPS::NO_GPS) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; - } -#if OPTFLOW == ENABLED - if (optflow.enabled()) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } -#endif - if (geofence_present()) { - control_sensors_present |= MAV_SYS_STATUS_GEOFENCE; - } - - if (aparm.throttle_min < 0) { - control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR; - } - if (plane.DataFlash.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 and motor 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); - - if (airspeed.enabled() && airspeed.use()) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } - - if (geofence_enabled()) { - control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE; - } - - if (plane.DataFlash.logging_enabled()) { - control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - } - - switch (control_mode) { - case MANUAL: - break; - - case ACRO: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control - break; - - case STABILIZE: - case FLY_BY_WIRE_A: - case AUTOTUNE: - case QSTABILIZE: - case QHOVER: - case QLAND: - case QLOITER: - 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 - break; - - case FLY_BY_WIRE_B: - case CRUISE: - 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 - break; - - case TRAINING: - if (!training_manual_roll || !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 - } - break; - - case AUTO: - case RTL: - case LOITER: - case AVOID_ADSB: - case GUIDED: - case CIRCLE: - case QRTL: - 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 - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; // altitude control - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control - break; - - case INITIALISING: - break; - } - - // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED) - if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; - } - - // default: all present sensors healthy except baro, 3D_MAG, GPS, DIFFERNTIAL_PRESSURE. GEOFENCE always defaults to healthy. - control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | - MAV_SYS_STATUS_SENSOR_3D_MAG | - MAV_SYS_STATUS_SENSOR_GPS | - MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE); - control_sensors_health |= MAV_SYS_STATUS_GEOFENCE; - - if (ahrs.initialised() && !ahrs.healthy()) { - // AHRS subsystem is unhealthy - control_sensors_health &= ~MAV_SYS_STATUS_AHRS; - } - 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; - } - - if (barometer.all_healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } - if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; - } - if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; - } -#if OPTFLOW == ENABLED - if (optflow.healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } -#endif - if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; - } - if (!ins.get_accel_health_all()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL; - } - if (airspeed.healthy()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; - } -#if GEOFENCE_ENABLED - if (geofence_breached()) { - control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE; - } -#endif - - if (plane.DataFlash.logging_failed()) { - control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; - } - - if (millis() - 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; - } - int16_t battery_current = -1; int8_t battery_remaining = -1; @@ -291,45 +129,8 @@ void Plane::send_extended_status1(mavlink_channel_t chan) battery_current = battery.current_amps() * 100; } -#if AP_TERRAIN_AVAILABLE - switch (terrain.status()) { - case AP_Terrain::TerrainStatusDisabled: - break; - case AP_Terrain::TerrainStatusUnhealthy: - control_sensors_present |= MAV_SYS_STATUS_TERRAIN; - control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; - break; - case AP_Terrain::TerrainStatusOK: - control_sensors_present |= MAV_SYS_STATUS_TERRAIN; - control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; - control_sensors_health |= MAV_SYS_STATUS_TERRAIN; - break; - } -#endif - -#if RANGEFINDER_ENABLED == ENABLED - if (rangefinder.num_sensors() > 0) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (g.rangefinder_landing) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - if (rangefinder.has_data()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - } -#endif - - if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) { - control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR; - control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR; - } - - if (AP_Notify::flags.initialising) { - // 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); - } - + update_sensor_status_flags(); + mavlink_msg_sys_status_send( chan, control_sensors_present, @@ -342,12 +143,6 @@ void Plane::send_extended_status1(mavlink_channel_t chan) 0, // comm drops %, 0, // comm drops in pkts, 0, 0, 0, 0); - -#if FRSKY_TELEM_ENABLED == ENABLED - // give mask of error flags to Frsky_Telemetry - uint32_t sensors_error_flags = (~control_sensors_health) & control_sensors_enabled & control_sensors_present; - frsky_telemetry.update_sensor_status_flags(sensors_error_flags); -#endif } void Plane::send_location(mavlink_channel_t chan) diff --git a/ArduPlane/GCS_Mavlink.h b/ArduPlane/GCS_Mavlink.h index 1dbd4a4589..bf17124f76 100644 --- a/ArduPlane/GCS_Mavlink.h +++ b/ArduPlane/GCS_Mavlink.h @@ -2,6 +2,9 @@ #include +// default sensors are present and healthy: gyro, accelerometer, barometer, rate_control, attitude_stabilization, yaw_position, altitude control, x/y position control, motor_control +#define MAVLINK_SENSOR_PRESENT_DEFAULT (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL | MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | 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_AHRS | MAV_SYS_STATUS_SENSOR_RC_RECEIVER) + class GCS_MAVLINK_Plane : public GCS_MAVLINK { diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 160db0a083..1b90ab536d 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -423,6 +423,11 @@ private: AP_Frsky_Telem frsky_telemetry {ahrs, battery, rangefinder}; #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; @@ -824,6 +829,7 @@ private: void send_heartbeat(mavlink_channel_t chan); void send_attitude(mavlink_channel_t chan); void send_fence_status(mavlink_channel_t chan); + void update_sensor_status_flags(void); void send_extended_status1(mavlink_channel_t chan); void send_location(mavlink_channel_t chan); void send_nav_controller_output(mavlink_channel_t chan); diff --git a/ArduPlane/sensors.cpp b/ArduPlane/sensors.cpp index 2503216bd7..6ddfe4cc61 100644 --- a/ArduPlane/sensors.cpp +++ b/ArduPlane/sensors.cpp @@ -170,3 +170,207 @@ void Plane::ice_update(void) { g2.ice_control.update(); } +// update error mask of sensors and subsystems. The mask +// 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) +{ + // default sensors present + control_sensors_present = MAVLINK_SENSOR_PRESENT_DEFAULT; + + // first what sensors/controllers we have + if (g.compass_enabled) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present + } + + if (airspeed.enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } + if (gps.status() > AP_GPS::NO_GPS) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_GPS; + } +#if OPTFLOW == ENABLED + if (optflow.enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif + if (geofence_present()) { + control_sensors_present |= MAV_SYS_STATUS_GEOFENCE; + } + + if (aparm.throttle_min < 0) { + control_sensors_present |= MAV_SYS_STATUS_REVERSE_MOTOR; + } + if (plane.DataFlash.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 and motor 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); + + if (airspeed.enabled() && airspeed.use()) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } + + if (geofence_enabled()) { + control_sensors_enabled |= MAV_SYS_STATUS_GEOFENCE; + } + + if (plane.DataFlash.logging_enabled()) { + control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; + } + + switch (control_mode) { + case MANUAL: + break; + + case ACRO: + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_ANGULAR_RATE_CONTROL; // 3D angular rate control + break; + + case STABILIZE: + case FLY_BY_WIRE_A: + case AUTOTUNE: + case QSTABILIZE: + case QHOVER: + case QLAND: + case QLOITER: + 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 + break; + + case FLY_BY_WIRE_B: + case CRUISE: + 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 + break; + + case TRAINING: + if (!training_manual_roll || !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 + } + break; + + case AUTO: + case RTL: + case LOITER: + case AVOID_ADSB: + case GUIDED: + case CIRCLE: + case QRTL: + 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 + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_YAW_POSITION; // yaw position + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; // altitude control + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; // X/Y position control + break; + + case INITIALISING: + break; + } + + // set motors outputs as enabled if safety switch is not disarmed (i.e. either NONE or ARMED) + if (hal.util->safety_switch_state() != AP_HAL::Util::SAFETY_DISARMED) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS; + } + + // default: all present sensors healthy except baro, 3D_MAG, GPS, DIFFERNTIAL_PRESSURE. GEOFENCE always defaults to healthy. + control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE | + MAV_SYS_STATUS_SENSOR_3D_MAG | + MAV_SYS_STATUS_SENSOR_GPS | + MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE); + control_sensors_health |= MAV_SYS_STATUS_GEOFENCE; + + if (ahrs.initialised() && !ahrs.healthy()) { + // AHRS subsystem is unhealthy + control_sensors_health &= ~MAV_SYS_STATUS_AHRS; + } + 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; + } + + if (barometer.all_healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } + if (g.compass_enabled && compass.healthy(0) && ahrs.use_compass()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; + } + if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; + } +#if OPTFLOW == ENABLED + if (optflow.healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif + if (!ins.get_gyro_health_all() || !ins.gyro_calibrated_ok_all()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_GYRO; + } + if (!ins.get_accel_health_all()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_ACCEL; + } + if (airspeed.healthy()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; + } +#if GEOFENCE_ENABLED + if (geofence_breached()) { + control_sensors_health &= ~MAV_SYS_STATUS_GEOFENCE; + } +#endif + + if (plane.DataFlash.logging_failed()) { + control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; + } + + if (millis() - 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()) { + case AP_Terrain::TerrainStatusDisabled: + break; + case AP_Terrain::TerrainStatusUnhealthy: + control_sensors_present |= MAV_SYS_STATUS_TERRAIN; + control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; + break; + case AP_Terrain::TerrainStatusOK: + control_sensors_present |= MAV_SYS_STATUS_TERRAIN; + control_sensors_enabled |= MAV_SYS_STATUS_TERRAIN; + control_sensors_health |= MAV_SYS_STATUS_TERRAIN; + break; + } +#endif + +#if RANGEFINDER_ENABLED == ENABLED + if (rangefinder.num_sensors() > 0) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (g.rangefinder_landing) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + if (rangefinder.has_data()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + } +#endif + + if (aparm.throttle_min < 0 && channel_throttle->get_servo_out() < 0) { + control_sensors_enabled |= MAV_SYS_STATUS_REVERSE_MOTOR; + control_sensors_health |= MAV_SYS_STATUS_REVERSE_MOTOR; + } + + if (AP_Notify::flags.initialising) { + // 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); +#endif +}