From a90182b9d8f2bb273d9aa1bbac6fefb672a4199f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sun, 9 Sep 2012 19:36:35 +1000
Subject: [PATCH] APM: fixed throttle nudging

The nudge value depended on RC3_TRIM, which is not a reliable
value. It tried to only take effect when the throttle stick was above
50%, but if RC3_TRIM was high for some reason (say 1500) then it would
actually depress the throttle by a large amount, which could cause the
plane to stall.

This also adds a boolean option THROTTLE_NUDGE to allow disabling of
throttle nudging
---
 ArduPlane/Parameters.h   | 2 ++
 ArduPlane/Parameters.pde | 8 ++++++++
 ArduPlane/radio.pde      | 7 ++++---
 3 files changed, 14 insertions(+), 3 deletions(-)

diff --git a/ArduPlane/Parameters.h b/ArduPlane/Parameters.h
index 08bbfc5ad..d36f6981d 100644
--- a/ArduPlane/Parameters.h
+++ b/ArduPlane/Parameters.h
@@ -75,6 +75,7 @@ public:
         k_param_land_flare_sec,
         k_param_crosstrack_min_distance,
         k_param_rudder_steer,
+        k_param_throttle_nudge,
 
         // 110: Telemetry control
         //
@@ -276,6 +277,7 @@ public:
     AP_Int8 throttle_fs_enabled;
     AP_Int16 throttle_fs_value;
     AP_Int8 throttle_cruise;
+    AP_Int8 throttle_nudge;
 
     // Failsafe
     AP_Int8 short_fs_action;
diff --git a/ArduPlane/Parameters.pde b/ArduPlane/Parameters.pde
index 42ca1e2e6..cbfdaa172 100644
--- a/ArduPlane/Parameters.pde
+++ b/ArduPlane/Parameters.pde
@@ -295,6 +295,14 @@ const AP_Param::Info var_info[] PROGMEM = {
     // @User: Standard
     GSCALAR(throttle_cruise,        "TRIM_THROTTLE",  THROTTLE_CRUISE),
 
+    // @Param: THROTTLE_NUDGE
+    // @DisplayName: Throttle nudge enable
+    // @Description: When enabled, this uses the throttle input in auto-throttle modes to 'nudge' the throttle to higher or lower values
+    // @Values: 0:Disabled,1:Enabled
+    // @User: Standard
+    // @User: Standard
+    GSCALAR(throttle_nudge,         "THROTTLE_NUDGE",  1),
+
     // @Param: FS_SHORT_ACTN
     // @DisplayName: Short failsafe action
     // @Description: The action to take on a short (1 second) failsafe event
diff --git a/ArduPlane/radio.pde b/ArduPlane/radio.pde
index 0ebe4e5c3..c5347b553 100644
--- a/ArduPlane/radio.pde
+++ b/ArduPlane/radio.pde
@@ -94,11 +94,12 @@ static void read_radio()
 
     g.channel_throttle.servo_out = g.channel_throttle.control_in;
 
-    if (g.channel_throttle.servo_out > 50) {
+    if (g.throttle_nudge && g.channel_throttle.servo_out > 50) {
+        float nudge = (g.channel_throttle.servo_out - 50) * 0.02;
         if (alt_control_airspeed()) {
-            airspeed_nudge_cm = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
+            airspeed_nudge_cm = (g.flybywire_airspeed_max * 100 - g.airspeed_cruise_cm) * nudge;
         } else {
-            throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((g.channel_throttle.norm_input()-0.5) / 0.5);
+            throttle_nudge = (g.throttle_max - g.throttle_cruise) * nudge;
         }
     } else {
         airspeed_nudge_cm = 0;
-- 
GitLab