From dcbd0fbd4488a4cf782522418d0c620327862579 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 27 Aug 2012 16:26:53 +1000
Subject: [PATCH] APM: cleanup throttle suppression code and don't use airspeed
 to unsupress

this removes the throttle suppression when any of the conditions are
met once, as otherwise flying slow below 10m could zero the throttle.

It also removes the use of airspeed for disabling throttle
supression. Otherwise a strong gust of wind can cause ArduPlane to
try to takeoff!
---
 ArduPlane/ArduPlane.pde |  3 ++
 ArduPlane/Attitude.pde  | 66 ++++++++++++++++++++++++++++++-----------
 ArduPlane/system.pde    |  6 ++++
 3 files changed, 58 insertions(+), 17 deletions(-)

diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde
index bf8e3f8714..82c6b8df80 100644
--- a/ArduPlane/ArduPlane.pde
+++ b/ArduPlane/ArduPlane.pde
@@ -524,6 +524,9 @@ static int32_t takeoff_altitude;
 // Minimum pitch to hold during takeoff command execution.  Hundredths of a degree
 static int16_t takeoff_pitch_cd;
 
+// this controls throttle suppression in auto modes
+static bool throttle_suppressed;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Loiter management
 ////////////////////////////////////////////////////////////////////////////////
diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index 10611c8ebb..ba75827fca 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -161,7 +161,7 @@ static void calc_throttle()
         g.channel_throttle.servo_out += (g.channel_pitch.servo_out * g.kff_pitch_to_throttle);
 
         g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out,
-                                                 g.throttle_min.get(), g.throttle_max.get()); // TODO - resolve why "saved" is used here versus "current"
+                                                 g.throttle_min.get(), g.throttle_max.get());
     }
 
 }
@@ -285,6 +285,53 @@ static void reset_I(void)
 //	g.pidAltitudeThrottle.reset_I();
 }
 
+/* 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:
+   *       1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
+   *       AND
+   *       2 - Our reported altitude is within 10 meters of the home altitude.
+   *       3 - Our reported speed is under 5 meters per second.
+   *       4 - We are not performing a takeoff in Auto mode
+   *       OR
+   *       5 - Home location is not set
+*/
+static bool suppress_throttle(void)
+{
+    if (!throttle_suppressed) {
+        // we've previously met a condition for unsupressing the throttle
+        return false;
+    }
+    if (control_mode != CIRCLE && control_mode <= FLY_BY_WIRE_A) {
+        // the user controls the throttle
+        throttle_suppressed = false;
+        return false;
+    }
+
+    if (control_mode==AUTO && takeoff_complete == false) {
+        // we're in auto takeoff 
+        throttle_suppressed = false;
+        return false;
+    }
+    
+    if (labs(home.alt - current_loc.alt) >= 1000) {
+        // we're more than 10m from the home altitude
+        throttle_suppressed = false;
+        return false;
+    }
+
+    if (g_gps != NULL && 
+        g_gps->status() == GPS::GPS_OK && 
+        g_gps->ground_speed >= 500) {
+        // we're moving at more than 5 m/s
+        throttle_suppressed = false;
+        return false;        
+    }
+
+    // throttle remains suppressed
+    return true;
+}
+
 /*****************************************
 * Set the flight control servos based on the current calculated values
 *****************************************/
@@ -355,22 +402,7 @@ static void set_servos(void)
         // convert 0 to 100% into PWM
         g.channel_throttle.servo_out = constrain(g.channel_throttle.servo_out, g.throttle_min.get(), g.throttle_max.get());
 
-        // 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:
-         *       1 - We are in Circle mode (which we use for short term failsafe), or in FBW-B or higher
-         *       AND
-         *       2 - Our reported altitude is within 10 meters of the home altitude.
-         *       3 - Our reported speed is under 5 meters per second.
-         *       4 - We are not performing a takeoff in Auto mode
-         *       OR
-         *       5 - Home location is not set
-         */
-        if (
-            (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) &&
-            (labs(home.alt - current_loc.alt) < 1000) &&
-            ((airspeed.use() ? airspeed.get_airspeed_cm() : g_gps->ground_speed) < 500 ) &&
-            !(control_mode==AUTO && takeoff_complete == false)
-            ) {
+        if (suppress_throttle()) {
             g.channel_throttle.servo_out = 0;
             g.channel_throttle.calc_pwm();
         }
diff --git a/ArduPlane/system.pde b/ArduPlane/system.pde
index 0d0bb44630..5d2f80dcbd 100644
--- a/ArduPlane/system.pde
+++ b/ArduPlane/system.pde
@@ -379,6 +379,12 @@ static void set_mode(byte mode)
         break;
     }
 
+    // if in an auto-throttle mode, start with throttle suppressed for
+    // safety. suppress_throttle() will unsupress it when appropriate
+    if (control_mode == CIRCLE || control_mode >= FLY_BY_WIRE_B) {
+        throttle_suppressed = true;
+    }
+
     if (g.log_bitmask & MASK_LOG_MODE)
         Log_Write_Mode(control_mode);
 }