From 9a0a83f40423b87dd55429397752d6901d02bffc Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Wed, 8 Oct 2014 20:18:37 +0900 Subject: [PATCH] Copter: report gyro unhealthy if failed calibration --- ArduCopter/GCS_Mavlink.pde | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 318d215467..374ee772f4 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -206,7 +206,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()) {