From fa326de7faed6faa5c8b8faff294ff8e5bc13ffd Mon Sep 17 00:00:00 2001 From: Tom Pittenger Date: Fri, 12 Aug 2016 12:20:13 -0700 Subject: [PATCH] Plane: add adsb to failsafe structure and report in heartbeat to GCS --- ArduPlane/GCS_Mavlink.cpp | 2 +- ArduPlane/Plane.h | 3 +++ 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/ArduPlane/GCS_Mavlink.cpp b/ArduPlane/GCS_Mavlink.cpp index 1ec1b15f74..68fdaed0d9 100644 --- a/ArduPlane/GCS_Mavlink.cpp +++ b/ArduPlane/GCS_Mavlink.cpp @@ -14,7 +14,7 @@ void Plane::send_heartbeat(mavlink_channel_t chan) uint8_t system_status; uint32_t custom_mode = control_mode; - if (failsafe.state != FAILSAFE_NONE || failsafe.low_battery) { + if (failsafe.state != FAILSAFE_NONE || failsafe.low_battery || failsafe.adsb) { system_status = MAV_STATE_CRITICAL; } else if (plane.crash_state.is_crashed) { system_status = MAV_STATE_EMERGENCY; diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 00f3c65ff6..fe1da8436b 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -350,6 +350,9 @@ private: // flag to hold whether battery low voltage threshold has been breached uint8_t low_battery:1; + // true if an adsb related failsafe has occurred + uint8_t adsb:1; + // saved flight mode enum FlightMode saved_mode;