From 34718b130a74828af45727d35cc67f9934798fcd Mon Sep 17 00:00:00 2001 From: floaledm Date: Tue, 20 Sep 2016 14:42:24 -0500 Subject: [PATCH] Copter: update sensor status error flags independently of sending a sys_status message Without this, there is no update to the sensor status flags in the Frsky lib unless there's an active Mavlink connection configured to send extended_status1 --- ArduCopter/ArduCopter.cpp | 6 ++ ArduCopter/Copter.h | 6 ++ ArduCopter/GCS_Mavlink.cpp | 167 ------------------------------------- ArduCopter/GCS_Mavlink.h | 3 + ArduCopter/sensors.cpp | 164 ++++++++++++++++++++++++++++++++++++ 5 files changed, 179 insertions(+), 167 deletions(-) diff --git a/ArduCopter/ArduCopter.cpp b/ArduCopter/ArduCopter.cpp index 8b8a45d1fa..1668f53037 100644 --- a/ArduCopter/ArduCopter.cpp +++ b/ArduCopter/ArduCopter.cpp @@ -502,6 +502,12 @@ void Copter::one_hz_loop() terrain_logging(); adsb.set_is_flying(!ap.land_complete); + + // 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(); } // called at 50hz diff --git a/ArduCopter/Copter.h b/ArduCopter/Copter.h index a5ec15d813..dc583d653f 100644 --- a/ArduCopter/Copter.h +++ b/ArduCopter/Copter.h @@ -449,6 +449,11 @@ private: AP_Frsky_Telem frsky_telemetry; #endif + // Variables for extended status MAVLink messages + uint32_t control_sensors_present; + uint32_t control_sensors_enabled; + uint32_t control_sensors_health; + // Altitude // The cm/s we are moving up or down based on filtered data - Positive = UP int16_t climb_rate; @@ -949,6 +954,7 @@ private: void failsafe_disable(); void fence_check(); void fence_send_mavlink_status(mavlink_channel_t chan); + void update_sensor_status_flags(void); bool set_mode(control_mode_t mode, mode_reason_t reason); bool gcs_set_mode(uint8_t mode) { return set_mode((control_mode_t)mode, MODE_REASON_GCS_COMMAND); } void update_flight_mode(); diff --git a/ArduCopter/GCS_Mavlink.cpp b/ArduCopter/GCS_Mavlink.cpp index 0e6dd2ebaa..c00aff2636 100644 --- a/ArduCopter/GCS_Mavlink.cpp +++ b/ArduCopter/GCS_Mavlink.cpp @@ -3,9 +3,6 @@ #include "GCS_Mavlink.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) - void Copter::gcs_send_heartbeat(void) { gcs_send_message(MSG_HEARTBEAT); @@ -111,130 +108,6 @@ NOINLINE void Copter::send_limits_status(mavlink_channel_t chan) NOINLINE void Copter::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 (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 PRECISION_LANDING == ENABLED - if (precland.enabled()) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; - } -#endif - if (ap.rc_receiver_present) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } - if (copter.DataFlash.logging_present()) { // primary logging only (usually File) - control_sensors_present |= MAV_SYS_STATUS_LOGGING; - } -#if PROXIMITY_ENABLED == ENABLED - if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } -#endif - - // all present sensors enabled by default except altitude and position control and motors which we will set individually - control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & - ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & - ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & - ~MAV_SYS_STATUS_LOGGING); - - switch (control_mode) { - case AUTO: - case AVOID_ADSB: - case GUIDED: - case LOITER: - case RTL: - case CIRCLE: - case LAND: - case POSHOLD: - case BRAKE: - case THROW: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; - break; - case ALT_HOLD: - case GUIDED_NOGPS: - case SPORT: - case AUTOTUNE: - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; - break; - default: - // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual) - 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; - } - - if (copter.DataFlash.logging_enabled()) { - control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - } - - - // default to all healthy - control_sensors_health = control_sensors_present; - - if (!barometer.all_healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; - } - if (!g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG; - } - if (gps.status() == AP_GPS::NO_GPS) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS; - } - if (!ap.rc_receiver_present || failsafe.radio) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER; - } -#if OPTFLOW == ENABLED - if (!optflow.healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; - } -#endif -#if PRECISION_LANDING == ENABLED - if (!precland.healthy()) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; - } -#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 (ahrs.initialised() && !ahrs.healthy()) { - // AHRS subsystem is unhealthy - control_sensors_health &= ~MAV_SYS_STATUS_AHRS; - } - - if (copter.DataFlash.logging_failed()) { - control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; - } - -#if PROXIMITY_ENABLED == ENABLED - if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) { - control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } -#endif - int16_t battery_current = -1; int8_t battery_remaining = -1; @@ -243,39 +116,6 @@ NOINLINE void Copter::send_extended_status1(mavlink_channel_t chan) battery_current = battery.current_amps() * 100; } -#if AP_TERRAIN_AVAILABLE && AC_TERRAIN - switch (terrain.status()) { - case AP_Terrain::TerrainStatusDisabled: - break; - case AP_Terrain::TerrainStatusUnhealthy: - // To-Do: restore unhealthy terrain status reporting once terrain is used in copter - //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; - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (rangefinder.has_data()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - } -#endif - - if (!ap.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); - } - mavlink_msg_sys_status_send( chan, control_sensors_present, @@ -288,13 +128,6 @@ NOINLINE void Copter::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 NOINLINE Copter::send_location(mavlink_channel_t chan) diff --git a/ArduCopter/GCS_Mavlink.h b/ArduCopter/GCS_Mavlink.h index f96da3e349..18c8703478 100644 --- a/ArduCopter/GCS_Mavlink.h +++ b/ArduCopter/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) + class GCS_MAVLINK_Copter : public GCS_MAVLINK { diff --git a/ArduCopter/sensors.cpp b/ArduCopter/sensors.cpp index e2d8182155..3511b691d9 100644 --- a/ArduCopter/sensors.cpp +++ b/ArduCopter/sensors.cpp @@ -265,3 +265,167 @@ void Copter::update_proximity(void) #endif } +// 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 Copter::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 (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 PRECISION_LANDING == ENABLED + if (precland.enabled()) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_VISION_POSITION; + } +#endif + if (ap.rc_receiver_present) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } + if (copter.DataFlash.logging_present()) { // primary logging only (usually File) + control_sensors_present |= MAV_SYS_STATUS_LOGGING; + } +#if PROXIMITY_ENABLED == ENABLED + if (copter.g2.proximity.get_status() > AP_Proximity::Proximity_NotConnected) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } +#endif + + // all present sensors enabled by default except altitude and position control and motors which we will set individually + control_sensors_enabled = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL & + ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & + ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & + ~MAV_SYS_STATUS_LOGGING); + + switch (control_mode) { + case AUTO: + case AVOID_ADSB: + case GUIDED: + case LOITER: + case RTL: + case CIRCLE: + case LAND: + case POSHOLD: + case BRAKE: + case THROW: + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL; + break; + case ALT_HOLD: + case GUIDED_NOGPS: + case SPORT: + case AUTOTUNE: + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_Z_ALTITUDE_CONTROL; + break; + default: + // stabilize, acro, drift, and flip have no automatic x,y or z control (i.e. all manual) + 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; + } + + if (copter.DataFlash.logging_enabled()) { + control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; + } + + + // default to all healthy + control_sensors_health = control_sensors_present; + + if (!barometer.all_healthy()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_ABSOLUTE_PRESSURE; + } + if (!g.compass_enabled || !compass.healthy() || !ahrs.use_compass()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_3D_MAG; + } + if (gps.status() == AP_GPS::NO_GPS) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_GPS; + } + if (!ap.rc_receiver_present || failsafe.radio) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_RC_RECEIVER; + } +#if OPTFLOW == ENABLED + if (!optflow.healthy()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_OPTICAL_FLOW; + } +#endif +#if PRECISION_LANDING == ENABLED + if (!precland.healthy()) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_VISION_POSITION; + } +#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 (ahrs.initialised() && !ahrs.healthy()) { + // AHRS subsystem is unhealthy + control_sensors_health &= ~MAV_SYS_STATUS_AHRS; + } + + if (copter.DataFlash.logging_failed()) { + control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; + } + +#if PROXIMITY_ENABLED == ENABLED + if (copter.g2.proximity.get_status() < AP_Proximity::Proximity_Good) { + control_sensors_health &= ~MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } +#endif + +#if AP_TERRAIN_AVAILABLE && AC_TERRAIN + switch (terrain.status()) { + case AP_Terrain::TerrainStatusDisabled: + break; + case AP_Terrain::TerrainStatusUnhealthy: + // To-Do: restore unhealthy terrain status reporting once terrain is used in copter + //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; + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (rangefinder.has_data()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + } +#endif + + if (!ap.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); +#endif +}