From 61e5b09f28b8ab2887561b17b492f6a390813edd Mon Sep 17 00:00:00 2001 From: Michael Oborne Date: Sat, 9 Feb 2013 10:11:36 +0800 Subject: [PATCH] Send FS state to gcs for AC --- ArduCopter/GCS_Mavlink.pde | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ArduCopter/GCS_Mavlink.pde b/ArduCopter/GCS_Mavlink.pde index 7381c5f91a..f48640350d 100644 --- a/ArduCopter/GCS_Mavlink.pde +++ b/ArduCopter/GCS_Mavlink.pde @@ -67,7 +67,11 @@ static NOINLINE void send_heartbeat(mavlink_channel_t chan) uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; uint8_t system_status = MAV_STATE_ACTIVE; uint32_t custom_mode = control_mode; - + + if (ap.failsafe == true) { + system_status = MAV_STATE_CRITICAL; + } + // work out the base_mode. This value is not very useful // for APM, but we calculate it as best we can so a generic // MAVLink enabled ground station can work out something about