From 02605591477cf5db2ba7a29c4bd70489f7457753 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 28 Nov 2012 12:13:09 +1100
Subject: [PATCH] Plane: fixed throttle slew limit code

starting at 1000 is a very bad idea - a petrol engine could cut out
---
 ArduPlane/Attitude.pde | 23 +++++++++++++----------
 1 file changed, 13 insertions(+), 10 deletions(-)

diff --git a/ArduPlane/Attitude.pde b/ArduPlane/Attitude.pde
index 56e149cf3e..4266b91067 100644
--- a/ArduPlane/Attitude.pde
+++ b/ArduPlane/Attitude.pde
@@ -281,15 +281,17 @@ static void calc_nav_roll()
 /*****************************************
 * Throttle slew limit
 *****************************************/
-static void throttle_slew_limit()
+static void throttle_slew_limit(int16_t last_throttle)
 {
-    static int16_t last = 1000;
     // if slew limit rate is set to zero then do not slew limit
     if (g.throttle_slewrate) {                   
         // limit throttle change by the given percentage per second
         float temp = g.throttle_slewrate * G_Dt * 0.01 * fabs(g.channel_throttle.radio_max - g.channel_throttle.radio_min);
-        g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last - (int)temp, last + (int)temp);
-        last = g.channel_throttle.radio_out;
+        // allow a minimum change of 1 PWM per cycle
+        if (temp < 1) {
+            temp = 1;
+        }
+        g.channel_throttle.radio_out = constrain(g.channel_throttle.radio_out, last_throttle - temp, last_throttle + temp);
     }
 }
 
@@ -347,6 +349,7 @@ static bool suppress_throttle(void)
 static void set_servos(void)
 {
     int16_t flapSpeedSource = 0;
+    int16_t last_throttle = g.channel_throttle.radio_out;
 
     if(control_mode == MANUAL) {
         // do a direct pass through of radio values
@@ -420,12 +423,6 @@ static void set_servos(void)
             g.channel_pitch.radio_out =     elevon2_trim + (BOOL_TO_SIGN(g.reverse_ch2_elevon) * (ch2 * 500.0/ SERVO_MAX));
         }
 
-        if (control_mode >= FLY_BY_WIRE_B) {
-            /* only do throttle slew limiting in modes where throttle
-             *  control is automatic */
-            throttle_slew_limit();
-        }
-
 #if OBC_FAILSAFE == ENABLED
         // this is to allow the failsafe module to deliberately crash 
         // the plane. Only used in extreme circumstances to meet the
@@ -496,6 +493,12 @@ static void set_servos(void)
         }
     }
 
+    if (control_mode >= FLY_BY_WIRE_B) {
+        /* only do throttle slew limiting in modes where throttle
+         *  control is automatic */
+        throttle_slew_limit(last_throttle);
+    }
+
 #if HIL_MODE == HIL_MODE_DISABLED || HIL_SERVOS
     // send values to the PWM timers for output
     // ----------------------------------------