diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp index 286973b0ad..0dd7d376fb 100644 --- a/APMrover2/APMrover2.cpp +++ b/APMrover2/APMrover2.cpp @@ -330,6 +330,12 @@ void Rover::one_second_loop(void) } ins.set_raw_logging(should_log(MASK_LOG_IMU_RAW)); + + // 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 Rover::dataflash_periodic(void) diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp index de2fa5c21f..eb9cff806c 100644 --- a/APMrover2/GCS_Mavlink.cpp +++ b/APMrover2/GCS_Mavlink.cpp @@ -3,9 +3,6 @@ #include "GCS_Mavlink.h" -// default sensors are present and healthy: gyro, accelerometer, 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_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS) - void Rover::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; @@ -88,82 +85,6 @@ void Rover::send_attitude(mavlink_channel_t chan) void Rover::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 (rover.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 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_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING); - - switch (control_mode) { - case MANUAL: - case HOLD: - break; - - case LEARNING: - case STEERING: - 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 GUIDED: - 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_XY_POSITION_CONTROL; // X/Y position control - break; - - case INITIALISING: - break; - } - - if (rover.DataFlash.logging_enabled()) { - control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; - } - - // 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 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()) { - 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 (!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; - } - int16_t battery_current = -1; int8_t battery_remaining = -1; @@ -172,25 +93,7 @@ void Rover::send_extended_status1(mavlink_channel_t chan) battery_current = battery.current_amps() * 100; } - if (sonar.num_sensors() > 0) { - control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - if (g.sonar_trigger_cm > 0) { - control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - if (sonar.has_data()) { - control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; - } - } - - if (rover.DataFlash.logging_failed()) { - control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; - } - - 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, @@ -204,12 +107,6 @@ void Rover::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 Rover::send_location(mavlink_channel_t chan) diff --git a/APMrover2/GCS_Mavlink.h b/APMrover2/GCS_Mavlink.h index ac3eb34158..a733c93758 100644 --- a/APMrover2/GCS_Mavlink.h +++ b/APMrover2/GCS_Mavlink.h @@ -2,6 +2,9 @@ #include +// default sensors are present and healthy: gyro, accelerometer, 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_ANGULAR_RATE_CONTROL | MAV_SYS_STATUS_SENSOR_ATTITUDE_STABILIZATION | MAV_SYS_STATUS_SENSOR_YAW_POSITION | MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL | MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS | MAV_SYS_STATUS_AHRS) + class GCS_MAVLINK_Rover : public GCS_MAVLINK { diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h index 155c74b01c..33df4e7eea 100644 --- a/APMrover2/Rover.h +++ b/APMrover2/Rover.h @@ -290,6 +290,10 @@ private: AP_Frsky_Telem frsky_telemetry; #endif + uint32_t control_sensors_present; + uint32_t control_sensors_enabled; + uint32_t control_sensors_health; + // Navigation control variables // The instantaneous desired lateral acceleration in m/s/s float lateral_acceleration; @@ -402,6 +406,7 @@ private: void update_navigation(); void send_heartbeat(mavlink_channel_t chan); void send_attitude(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/APMrover2/sensors.cpp b/APMrover2/sensors.cpp index 1457674319..a0e63ffe85 100644 --- a/APMrover2/sensors.cpp +++ b/APMrover2/sensors.cpp @@ -125,3 +125,107 @@ void Rover::button_update(void) { button.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 Rover::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 (rover.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 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_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS & ~MAV_SYS_STATUS_LOGGING); + + switch (control_mode) { + case MANUAL: + case HOLD: + break; + + case LEARNING: + case STEERING: + 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 GUIDED: + 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_XY_POSITION_CONTROL; // X/Y position control + break; + + case INITIALISING: + break; + } + + if (rover.DataFlash.logging_enabled()) { + control_sensors_enabled |= MAV_SYS_STATUS_LOGGING; + } + + // 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 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()) { + 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 (!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 (sonar.num_sensors() > 0) { + control_sensors_present |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + if (g.sonar_trigger_cm > 0) { + control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + if (sonar.has_data()) { + control_sensors_health |= MAV_SYS_STATUS_SENSOR_LASER_POSITION; + } + } + + if (rover.DataFlash.logging_failed()) { + control_sensors_health &= ~MAV_SYS_STATUS_LOGGING; + } + + 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 +}