From 6d0ad051922ff7bee7247ada78d0fe64db79c4ac Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Wed, 29 Jul 2015 14:44:44 -0700 Subject: [PATCH] GCS_Mavlink: added is_crashed to mavlink heartbeat status which shows EMERGENCY --- ArduPlane/GCS_Mavlink.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 04ad2507ea..85618ccf26 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -8,11 +8,17 @@ void Plane::send_heartbeat(mavlink_channel_t chan) { uint8_t base_mode = MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; - uint8_t system_status = is_flying() ? MAV_STATE_ACTIVE : MAV_STATE_STANDBY; + uint8_t system_status; uint32_t custom_mode = control_mode; if (failsafe.state != FAILSAFE_NONE) { system_status = MAV_STATE_CRITICAL; + } else if (plane.auto_state.is_crashed) { + system_status = MAV_STATE_EMERGENCY; + } else if (is_flying()) { + system_status = MAV_STATE_ACTIVE; + } else { + system_status = MAV_STATE_STANDBY; } // work out the base_mode. This value is not very useful