From c2cdc0468bc92b451791be70a9b79a84c56953d2 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Mon, 1 Sep 2014 20:22:29 +0900 Subject: [PATCH] Plane: individual accel and gyro status to GCS --- ArduPlane/GCS_Mavlink.pde | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 6f00211323..f530b7092b 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -225,8 +225,11 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan) if (gps.status() >= AP_GPS::GPS_OK_FIX_3D) { 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 (!ins.get_gyro_health_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;