Copter: report gyro unhealthy if failed calibration

This commit is contained in:
Randy Mackay 2014-10-08 20:18:37 +09:00
parent eb594775b7
commit 661755e05a
1 changed files with 1 additions and 1 deletions

View File

@ -199,7 +199,7 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan)
if (ap.rc_receiver_present && !failsafe.radio) {
control_sensors_health |= MAV_SYS_STATUS_SENSOR_RC_RECEIVER;
}
if (!ins.get_gyro_health_all()) {
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()) {