diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index de055e3eb9..5fdddb9aed 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -153,6 +153,13 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack // at the moment all sensors/controllers are assumed healthy control_sensors_health = control_sensors_present; + if (!compass.healthy) { + control_sensors_health &= ~(1<<2); // compass + } + if (!compass.use_for_yaw()) { + control_sensors_enabled &= ~(1<<2); // compass + } + uint16_t battery_current = -1; uint8_t battery_remaining = -1; diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde index 53489ad0ed..8ddb8c4c67 100644 --- a/ArduPlane/GCS_Mavlink.pde +++ b/ArduPlane/GCS_Mavlink.pde @@ -190,6 +190,13 @@ static NOINLINE void send_extended_status1(mavlink_channel_t chan, uint16_t pack // at the moment all sensors/controllers are assumed healthy control_sensors_health = control_sensors_present; + if (!compass.healthy) { + control_sensors_health &= ~(1<<2); // compass + } + if (!compass.use_for_yaw()) { + control_sensors_enabled &= ~(1<<2); // compass + } + uint16_t battery_current = -1; uint8_t battery_remaining = -1;