Rover: Fixed a bug in reverse

If a user was driving in reverse and when into an auto mission the
rover would try to do the whole mission in reverse.  This fixes that.

APMrover2/APMrover2.cpp

@@ -101,6 +101,8 @@ void Rover::setup()
     notify.init(false);
 
     AP_Notify::flags.failsafe_battery = false;
+
+    in_auto_reverse = false;
     
     rssi.init();
 
@@ -430,12 +432,18 @@ void Rover::update_current_mode(void)
     switch (control_mode){
     case AUTO:
     case RTL:
+        if (!in_auto_reverse) {
+            set_reverse(false);
+        }
         calc_lateral_acceleration();
         calc_nav_steer();
         calc_throttle(g.speed_cruise);
         break;
 
     case GUIDED: {
+        if (!in_auto_reverse) {
+            set_reverse(false);
+        }
         switch (guided_mode){
         case Guided_Angle:
             nav_set_yaw_speed();
@@ -502,6 +510,7 @@ void Rover::update_current_mode(void)
           we set the exact value in set_servos(), but it helps for
           logging
          */
+        in_auto_reverse = false;
         channel_throttle->set_servo_out(channel_throttle->get_control_in());
         channel_steer->set_servo_out(channel_steer->pwm_to_angle());
 
@@ -514,6 +523,9 @@ 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);
+        if (!in_auto_reverse) {
+            set_reverse(false);
+        }
         break;
 
     case INITIALISING:

APMrover2/Rover.h

@@ -351,6 +351,9 @@ private:
     // set if we are driving backwards
     bool in_reverse;
 
+    // set if the users asks for auto reverse
+    bool in_auto_reverse;
+
     static const AP_Scheduler::Task scheduler_tasks[];
 
     // use this to prevent recursion during sensor init

APMrover2/commands_logic.cpp

@@ -513,8 +513,10 @@ void Rover::log_picture()
 void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
 {
     if (cmd.p1 == 1) {
+        in_auto_reverse = true;
         set_reverse(true);
     } else {
+        in_auto_reverse = false;
         set_reverse(false);
     }
 }

APMrover2/system.cpp

@@ -277,6 +277,9 @@ void Rover::set_mode(enum mode mode)
     control_mode = mode;
     throttle_last = 0;
     throttle = 500;
+    if (!in_auto_reverse) {
+        set_reverse(false);
+    }
     g.pidSpeedThrottle.reset_I();
 
 #if FRSKY_TELEM_ENABLED == ENABLED