mirror of https://github.com/ArduPilot/ardupilot
Plane: update sensor status error flags independently of sending a sys_status message
This commit is contained in:
floaledm
Andrew Tridgell
parent
1aab5eec38
commit
bae9ce20c1
|
|
@ -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()
|
||||
|
|
|
|||
|
|
@ -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,44 +129,7 @@ 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,
|
||||
|
|
@ -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)
|
||||
|
|
|
|||
|
|
@ -2,6 +2,9 @@
|
|||
|
||||
#include <GCS_MAVLink/GCS.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)
|
||||
|
||||
class GCS_MAVLINK_Plane : public GCS_MAVLINK
|
||||
{
|
||||
|
||||
|
|
|
|||
|
|
@ -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);
|
||||
|
|
|
|||
|
|
@ -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
|
||||
}
|
||||
|
|
|
|||
Loading…
Reference in New Issue