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()
     // load the default values of variables listed in var_info[]
     AP_Param::setup_sketch_defaults();
 
+    in_auto_reverse = false;
+    
     rssi.init();
 
     init_ardupilot();
@@ -437,12 +439,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();
@@ -510,6 +518,7 @@ void Rover::update_current_mode(void)
           we set the exact value in set_servos(), but it helps for
           logging
          */
+        in_auto_reverse = false;
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,channel_throttle->get_control_in());
         SRV_Channels::set_output_scaled(SRV_Channel::k_steering,channel_steer->get_control_in());
 
@@ -522,6 +531,9 @@ void Rover::update_current_mode(void)
         // hold position - stop motors and center steering
         SRV_Channels::set_output_scaled(SRV_Channel::k_throttle,0);
         SRV_Channels::set_output_scaled(SRV_Channel::k_steering,0);
+        if (!in_auto_reverse) {
+            set_reverse(false);
+        }
         break;
 
     case INITIALISING:

APMrover2/Rover.h

@@ -360,6 +360,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

@@ -509,8 +509,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

@@ -284,6 +284,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