From 57c23c7fe3f56f348688b9a50fc716bf84612618 Mon Sep 17 00:00:00 2001
From: Grant Morphett <grant@gmorph.com>
Date: Thu, 14 Jul 2016 17:50:39 +1000
Subject: [PATCH] Rover: Adding support for rovers driving in Reverse.

---
 APMrover2/APMrover2.cpp      |  3 ---
 APMrover2/Rover.h            |  1 +
 APMrover2/Steering.cpp       |  4 +++-
 APMrover2/commands_logic.cpp | 13 +++++++++++++
 APMrover2/system.cpp         |  4 +++-
 5 files changed, 20 insertions(+), 5 deletions(-)

diff --git a/APMrover2/APMrover2.cpp b/APMrover2/APMrover2.cpp
index 0409f22e4d..5d12389850 100644
--- a/APMrover2/APMrover2.cpp
+++ b/APMrover2/APMrover2.cpp
@@ -411,14 +411,12 @@ void Rover::update_current_mode(void)
     switch (control_mode){
     case AUTO:
     case RTL:
-        set_reverse(false);
         calc_lateral_acceleration();
         calc_nav_steer();
         calc_throttle(g.speed_cruise);
         break;
 
     case GUIDED:
-        set_reverse(false);
         if (rtl_complete || verify_RTL()) {
             // we have reached destination so stop where we are
             if (channel_throttle->get_servo_out() != g.throttle_min.get()) {
@@ -483,7 +481,6 @@ void Rover::update_current_mode(void)
         // hold position - stop motors and center steering
         channel_throttle->set_servo_out(0);
         channel_steer->set_servo_out(0);
-        set_reverse(false);
         break;
 
     case INITIALISING:
diff --git a/APMrover2/Rover.h b/APMrover2/Rover.h
index 64fe5a5921..6b10ce9ef2 100644
--- a/APMrover2/Rover.h
+++ b/APMrover2/Rover.h
@@ -511,6 +511,7 @@ private:
     void do_set_home(const AP_Mission::Mission_Command& cmd);
     void do_digicam_configure(const AP_Mission::Mission_Command& cmd);
     void do_digicam_control(const AP_Mission::Mission_Command& cmd);
+    void do_set_reverse(const AP_Mission::Mission_Command& cmd);
     void init_capabilities(void);
     void rudder_arm_disarm_check();
     void change_arm_state(void);
diff --git a/APMrover2/Steering.cpp b/APMrover2/Steering.cpp
index f4c655e2b4..51ba2cd2ac 100644
--- a/APMrover2/Steering.cpp
+++ b/APMrover2/Steering.cpp
@@ -203,7 +203,9 @@ void Rover::calc_nav_steer() {
     }
 
     // add in obstacle avoidance
-    lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
+    if (!in_reverse) {
+        lateral_acceleration += (obstacle.turn_angle/45.0f) * g.turn_max_g;
+    }
 
     // constrain to max G force
     lateral_acceleration = constrain_float(lateral_acceleration, -g.turn_max_g*GRAVITY_MSS, g.turn_max_g*GRAVITY_MSS);
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 3581fd1690..975219186a 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -107,6 +107,10 @@ bool Rover::start_command(const AP_Mission::Mission_Command& cmd)
             break;
 #endif
 
+        case MAV_CMD_DO_SET_REVERSE:
+            do_set_reverse(cmd);
+            break;
+
 		default:
 		    // return false for unhandled commands
 		    return false;
@@ -405,3 +409,12 @@ void Rover::log_picture()
         }      
     }
 }
+
+void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
+{
+	if (cmd.p1 == 1) {
+        set_reverse(true);
+	} else {
+        set_reverse(false);
+    }
+}
diff --git a/APMrover2/system.cpp b/APMrover2/system.cpp
index fec3cae615..20515e1540 100644
--- a/APMrover2/system.cpp
+++ b/APMrover2/system.cpp
@@ -250,6 +250,9 @@ void Rover::set_reverse(bool reverse)
         return;
     }
     g.pidSpeedThrottle.reset_I();
+    steerController.reset_I();
+    nav_controller->set_reverse(reverse);
+    steerController.set_reverse(reverse);
     in_reverse = reverse;
 }
 
@@ -269,7 +272,6 @@ void Rover::set_mode(enum mode mode)
     control_mode = mode;
     throttle_last = 0;
     throttle = 500;
-    set_reverse(false);
     g.pidSpeedThrottle.reset_I();
 
     if (control_mode != AUTO) {