From 2c840547fa2b316af8b789d51fa9a0d5926323ae Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 25 Nov 2013 11:21:15 +1100
Subject: [PATCH] Rover: fixed throttle control in reverse

throttle nudge was being miscalculated
---
 APMrover2/APMrover2.pde |  4 ++--
 APMrover2/radio.pde     |  4 ++--
 APMrover2/system.pde    | 15 ++++++++++++++-
 3 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 25eb2ed94a..a0c8d385f1 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -869,7 +869,7 @@ static void update_current_mode(void)
         // and throttle gives speed in proportion to cruise speed, up
         // to 50% throttle, then uses nudging above that.
         float target_speed = channel_throttle->pwm_to_angle() * 0.01 * 2 * g.speed_cruise;
-        in_reverse = (target_speed < 0);
+        set_reverse(target_speed < 0);
         if (in_reverse) {
             target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
         } else {
@@ -892,7 +892,7 @@ static void update_current_mode(void)
 
         // mark us as in_reverse when using a negative throttle to
         // stop AHRS getting off
-        in_reverse = (channel_throttle->servo_out < 0);
+        set_reverse(channel_throttle->servo_out < 0);
         break;
 
     case HOLD:
diff --git a/APMrover2/radio.pde b/APMrover2/radio.pde
index 1962249ed4..2de38d456d 100644
--- a/APMrover2/radio.pde
+++ b/APMrover2/radio.pde
@@ -53,8 +53,8 @@ static void read_radio()
 
 	channel_throttle->servo_out = channel_throttle->control_in;
 
-	if (channel_throttle->servo_out > 50) {
-        throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((channel_throttle->norm_input()-0.5) / 0.5);
+	if (abs(channel_throttle->servo_out) > 50) {
+        throttle_nudge = (g.throttle_max - g.throttle_cruise) * ((fabsf(channel_throttle->norm_input())-0.5) / 0.5);
 	} else {
 		throttle_nudge = 0;
 	}
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index f49002a4d0..d1137906a8 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -242,6 +242,19 @@ static void startup_ground(void)
 	gcs_send_text_P(SEVERITY_LOW,PSTR("\n\n Ready to drive."));
 }
 
+/*
+  set the in_reverse flag
+  reset the throttle integrator if this changes in_reverse
+ */
+static void set_reverse(bool reverse)
+{
+    if (in_reverse == reverse) {
+        return;
+    }
+    g.pidSpeedThrottle.reset_I();    
+    in_reverse = reverse;
+}
+
 static void set_mode(enum mode mode)
 {       
 
@@ -252,7 +265,7 @@ static void set_mode(enum mode mode)
 	control_mode = mode;
     throttle_last = 0;
     throttle = 500;
-    in_reverse = false;
+    set_reverse(false);
     g.pidSpeedThrottle.reset_I();
 
     if (control_mode != AUTO) {