Copter: report INS health

This commit is contained in:
Andrew Tridgell 2013-10-30 09:02:24 +11:00
parent 9748e72b3e
commit b41f4898e0

View File

@ -194,6 +194,9 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack
if (ap.rc_receiver_present && !failsafe.radio) { if (ap.rc_receiver_present && !failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER; control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
} }
if (!ins.healthy()) {
control_sensors_health &= ~(MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_3D_ACCEL);
}
int16_t battery_current = -1; int16_t battery_current = -1;
int8_t battery_remaining = -1; int8_t battery_remaining = -1;