Rover: minor fixes for reversed handling

APMrover2/GCS_Mavlink.cpp

@@ -537,14 +537,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in
         }
         return MAV_RESULT_ACCEPTED;
 
-    case MAV_CMD_DO_SET_REVERSE:
-        // param1 : Direction (0=Forward, 1=Reverse)
-        if (is_equal(packet.param1, 1.0f)) {
-            rover.control_mode->set_reversed(1);
-            return MAV_RESULT_ACCEPTED;
-           }
-        return MAV_RESULT_FAILED;
-
     case MAV_CMD_DO_SET_HOME: {
         // assume failure
         if (is_equal(packet.param1, 1.0f)) {
@@ -587,6 +579,11 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_int_packet(const mavlink_command_in
         return MAV_RESULT_ACCEPTED;
     }
 
+    case MAV_CMD_DO_SET_REVERSE:
+        // param1 : Direction (0=Forward, 1=Reverse)
+        rover.control_mode->set_reversed(is_equal(packet.param1,1.0f));
+        return MAV_RESULT_ACCEPTED;
+
 #if MOUNT == ENABLED
     case MAV_CMD_DO_SET_ROI: {
         // param1 : /* Region of interest mode (not used)*/
@@ -727,6 +724,11 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
         return MAV_RESULT_FAILED;
     }
 
+    case MAV_CMD_DO_SET_REVERSE:
+        // param1 : Direction (0=Forward, 1=Reverse)
+        rover.control_mode->set_reversed(is_equal(packet.param1,1.0f));
+        return MAV_RESULT_ACCEPTED;
+
     case MAV_CMD_NAV_SET_YAW_SPEED:
     {
         // param1 : yaw angle to adjust direction by in centidegress

APMrover2/commands_logic.cpp

@@ -418,5 +418,5 @@ void Rover::do_digicam_control(const AP_Mission::Mission_Command& cmd)
 
 void Rover::do_set_reverse(const AP_Mission::Mission_Command& cmd)
 {
-        rover.control_mode->set_reversed(cmd.p1 == 1);
+    control_mode->set_reversed(cmd.p1 == 1);
 }

APMrover2/mode.cpp

@@ -229,9 +229,7 @@ bool Mode::set_desired_speed(float speed)
 // execute the mission in reverse (i.e. backing up)
 void Mode::set_reversed(bool value)
 {
-    if (_reversed != value) {
-        _reversed = value;
-    }
+    _reversed = value;
 }
 
 void Mode::calc_throttle(float target_speed, bool nudge_allowed, bool avoidance_enabled)