From 996eda0080f76be25e37b2c06efd4d02fa20082a Mon Sep 17 00:00:00 2001
From: Kevin Hester <kevinh@geeksville.com>
Date: Sat, 14 Sep 2013 15:09:02 -1000
Subject: [PATCH] Plane: Report MAV_STATE_ACTIVE if flying, MAV_STATE_STANDBY
 otherwise

(There are also various error states MAV_STATE_CRITICAL that are unchanged
by this check in).

The is_flying() check is similar to the code in suppress_throttle, but
I wanted to make it very optimistic on thinking we are flying - which is
not what you want for set_throttle...

(AC has similar MAV_STATE_ACTIVE vs MAV_STATE_STANDBY behavior now)
---
 ArduPlane/Attitude.pde    | 21 +++++++++++++++++++++
 ArduPlane/GCS_Mavlink.pde |  2 +-
 2 files changed, 22 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index 6805f0b31a..3a8ae42490 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -517,6 +517,27 @@ no_launch:
 }
 
 
+/**
+  Do we think we are flying?
+  This is a heuristic so it could be wrong in some cases.  In particular, if we don't have GPS lock we'll fall
+  back to only using altitude.  (This is probably more optimistic than what suppress_throttle wants...)
+*/
+static bool is_flying(void)
+{
+    // If we don't have a GPS lock then don't use GPS for this test
+    bool gpsMovement = (g_gps == NULL ||
+                        g_gps->status() < GPS::GPS_OK_FIX_2D ||
+                        g_gps->ground_speed_cm >= 500);
+    
+    bool airspeedMovement = !airspeed.use() || airspeed.get_airspeed() >= 5;
+    
+    // we're more than 5m from the home altitude
+    bool inAir = relative_altitude_abs_cm() > 500;
+
+    return inAir && gpsMovement && airspeedMovement;
+}
+
+
 /* We want to supress the throttle if we think we are on the ground and in an autopilot controlled throttle mode.
 
    Disable throttle if following conditions are met:
diff --git a/ArduPlane/GCS_Mavlink.pde b/ArduPlane/GCS_Mavlink.pde
index 800d11322a..b50d04a6c4 100644
--- a/ArduPlane/GCS_Mavlink.pde
+++ b/ArduPlane/GCS_Mavlink.pde
@@ -25,7 +25,7 @@ static bool	gcs_out_of_time;
 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;
+    uint8_t system_status = is_flying() ? MAV_STATE_ACTIVE : MAV_STATE_STANDBY;
     uint32_t custom_mode = control_mode;
     
     if (failsafe.state != FAILSAFE_NONE) {