aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Plane: report INS and airspeed health

Browse Source
This commit is contained in:
Andrew Tridgell 2013-10-30 09:03:20 +11:00 committed by Randy Mackay
parent f182169c01
commit b62ad0d5fb
1 changed files with 16 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

19
ArduPlane/GCS_Mavlink.pde
View File

@ -142,7 +142,8 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
if (g.compass_enabled) { if (g.compass_enabled) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present control_sensors_present |= MAV_SYS_STATUS_SENSOR_3D_MAG; // compass present
} }
if (airspeed.enabled() && airspeed.use()) {
if (airspeed.enabled()) {
control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE; control_sensors_present |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
} }
if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) { if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) {
@ -152,6 +153,10 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
// all present sensors enabled by default except rate control, attitude stabilization, yaw, altitude, position control and motor output which we will set individually // 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_Z_ALTITUDE_CONTROL & ~MAV_SYS_STATUS_SENSOR_XY_POSITION_CONTROL & ~MAV_SYS_STATUS_SENSOR_MOTOR_OUTPUTS); 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);
if (airspeed.enabled() && airspeed.use()) {
control_sensors_enabled |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
switch (control_mode) { switch (control_mode) {
case MANUAL: case MANUAL:
break; break;
@ -197,14 +202,22 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
break; break;
} }
// default to all healthy except compass and gps which we set individually // default to all healthy
control_sensors_health = control_sensors_present & (~MAV_SYS_STATUS_SENSOR_3D_MAG & ~MAV_SYS_STATUS_SENSOR_GPS); control_sensors_health = control_sensors_present & ~(MAV_SYS_STATUS_SENSOR_3D_MAG |
MAV_SYS_STATUS_SENSOR_GPS |
MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE);
if (g.compass_enabled && compass.healthy && ahrs.use_compass()) { if (g.compass_enabled && compass.healthy && ahrs.use_compass()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG; control_sensors_health |= MAV_SYS_STATUS_SENSOR_3D_MAG;
} }
if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) { if (g_gps != NULL && g_gps->status() > GPS::NO_GPS) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS; control_sensors_health |= MAV_SYS_STATUS_SENSOR_GPS;
} }
if (!ins.healthy()) {
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
if (airspeed.healthy()) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_DIFFERENTIAL_PRESSURE;
}
int16_t battery_current = -1; int16_t battery_current = -1;
int8_t battery_remaining = -1; int8_t battery_remaining = -1;