Rover: Adding support for rovers driving in Reverse.

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:

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);

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);

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);
+    }
+}

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) {