From 64d6d547573dcb5307f63ed46d7a46b78a9d693f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 18 Nov 2013 10:58:22 +1100
Subject: [PATCH] Rover: added basic support for reverse in STEERING mode

this will allow for reverse in steering mode, while also fixing a
problem with AHRS yaw when reversing, and a problem with initial
throttle in steering mode
---
 APMrover2/APMrover2.pde | 20 +++++++++++++++++++-
 APMrover2/Steering.pde  | 29 +++++++++++++++++------------
 APMrover2/system.pde    |  2 ++
 3 files changed, 38 insertions(+), 13 deletions(-)

diff --git a/APMrover2/APMrover2.pde b/APMrover2/APMrover2.pde
index 2af2bbebff..25eb2ed94a 100644
--- a/APMrover2/APMrover2.pde
+++ b/APMrover2/APMrover2.pde
@@ -537,6 +537,9 @@ static uint32_t		delta_us_fast_loop;
 // Counter of main loop executions.  Used for performance monitoring and failsafe processing
 static uint16_t			mainLoop_count;
 
+// set if we are driving backwards
+static bool in_reverse;
+
 ////////////////////////////////////////////////////////////////////////////////
 // Top-level logic
 ////////////////////////////////////////////////////////////////////////////////
@@ -636,6 +639,12 @@ static void ahrs_update()
     gcs_update();
 #endif
 
+    // when in reverse we need to tell AHRS not to assume we are a
+    // 'fly forward' vehicle, otherwise it will see a large
+    // discrepancy between the mag and the GPS heading and will try to
+    // correct for it, leading to a large yaw error
+    ahrs.set_fly_forward(!in_reverse);
+
     ahrs.update();
 
     if (g.log_bitmask & MASK_LOG_ATTITUDE_FAST)
@@ -860,7 +869,12 @@ 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;
-        target_speed = constrain_float(target_speed, 0, g.speed_cruise);
+        in_reverse = (target_speed < 0);
+        if (in_reverse) {
+            target_speed = constrain_float(target_speed, -g.speed_cruise, 0);
+        } else {
+            target_speed = constrain_float(target_speed, 0, g.speed_cruise);
+        }
         calc_throttle(target_speed);
         break;
     }
@@ -875,6 +889,10 @@ static void update_current_mode(void)
          */
         channel_throttle->servo_out = channel_throttle->control_in;
         channel_steer->servo_out = channel_steer->pwm_to_angle();
+
+        // mark us as in_reverse when using a negative throttle to
+        // stop AHRS getting off
+        in_reverse = (channel_throttle->servo_out < 0);
         break;
 
     case HOLD:
diff --git a/APMrover2/Steering.pde b/APMrover2/Steering.pde
index 0374b7cc84..912d130395 100644
--- a/APMrover2/Steering.pde
+++ b/APMrover2/Steering.pde
@@ -75,13 +75,8 @@ static void calc_throttle(float target_speed)
         return;
     }
 
-    if (target_speed <= 0) {
-        // cope with zero requested speed
-        channel_throttle->servo_out = g.throttle_min.get();
-        return;
-    }
-
-    int throttle_target = g.throttle_cruise + throttle_nudge;  
+    float throttle_base = (fabsf(target_speed) / g.speed_cruise) * g.throttle_cruise;
+    int throttle_target = throttle_base + throttle_nudge;  
 
     /*
       reduce target speed in proportion to turning rate, up to the
@@ -102,7 +97,7 @@ static void calc_throttle(float target_speed)
     // reduce the target speed by the reduction factor
     target_speed *= reduction;
 
-    groundspeed_error = target_speed - ground_speed; 
+    groundspeed_error = fabsf(target_speed) - ground_speed; 
     
     throttle = throttle_target + (g.pidSpeedThrottle.get_pid(groundspeed_error * 100) / 100);
 
@@ -110,7 +105,11 @@ static void calc_throttle(float target_speed)
     // much faster response in turns
     throttle *= reduction;
 
-    channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min.get(), g.throttle_max.get());
+    if (in_reverse) {
+        channel_throttle->servo_out = constrain_int16(-throttle, -g.throttle_max, -g.throttle_min);
+    } else {
+        channel_throttle->servo_out = constrain_int16(throttle, g.throttle_min, g.throttle_max);
+    }
 }
 
 /*****************************************
@@ -179,9 +178,15 @@ static void set_servos(void)
         }
 	} else {       
         channel_steer->calc_pwm();
-		channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
-                                                       g.throttle_min.get(), 
-                                                       g.throttle_max.get());
+        if (in_reverse) {
+            channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
+                                                          -g.throttle_max,
+                                                          -g.throttle_min);
+        } else {
+            channel_throttle->servo_out = constrain_int16(channel_throttle->servo_out, 
+                                                          g.throttle_min.get(), 
+                                                          g.throttle_max.get());
+        }
 
         if ((failsafe.bits & FAILSAFE_EVENT_THROTTLE) && control_mode < AUTO) {
             // suppress throttle if in failsafe
diff --git a/APMrover2/system.pde b/APMrover2/system.pde
index 34d217ed19..f49002a4d0 100644
--- a/APMrover2/system.pde
+++ b/APMrover2/system.pde
@@ -252,6 +252,8 @@ static void set_mode(enum mode mode)
 	control_mode = mode;
     throttle_last = 0;
     throttle = 500;
+    in_reverse = false;
+    g.pidSpeedThrottle.reset_I();
 
     if (control_mode != AUTO) {
         auto_triggered = false;