diff --git a/APMrover2/GCS_Mavlink.cpp b/APMrover2/GCS_Mavlink.cpp
index 77fe48bada..30e7768ca0 100644
--- a/APMrover2/GCS_Mavlink.cpp
+++ b/APMrover2/GCS_Mavlink.cpp
@@ -537,6 +537,14 @@ 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)) {
@@ -693,13 +701,6 @@ MAV_RESULT GCS_MAVLINK_Rover::handle_command_long_packet(const mavlink_command_l
         }
         return MAV_RESULT_ACCEPTED;
 
-    case MAV_CMD_DO_SET_REVERSE:
-        // param1 : Direction (0=Forward, 1=Reverse)
-        if(!rover.control_mode->set_reversed(packet.param1)) {
-            return MAV_RESULT_FAILED;
-        }
-        return MAV_RESULT_ACCEPTED;
-
     case MAV_CMD_DO_SET_HOME:
     {
         // param1 : use current (1=use current location, 0=use specified location)
diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index f5ac3bada9..84715816c6 100644
--- a/APMrover2/commands_logic.cpp
+++ b/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)
 {
-    mode_auto.set_reversed(cmd.p1 == 1);
+        rover.control_mode->set_reversed(cmd.p1 == 1);
 }
diff --git a/APMrover2/mode.h b/APMrover2/mode.h
index 4a75166e23..c5eece3139 100644
--- a/APMrover2/mode.h
+++ b/APMrover2/mode.h
@@ -251,7 +251,6 @@ public:
     // start RTL (within auto)
     void start_RTL();
 
-
 protected:
 
     bool _enter() override;
diff --git a/APMrover2/mode_guided.cpp b/APMrover2/mode_guided.cpp
index 335e422d06..c4759a2231 100644
--- a/APMrover2/mode_guided.cpp
+++ b/APMrover2/mode_guided.cpp
@@ -29,8 +29,8 @@ void ModeGuided::update()
             // determine if we should keep navigating
             if (!_reached_destination || (rover.is_boat() && !near_wp)) {
                 // drive towards destination
-                calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination);
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, true);
+                calc_steering_to_waypoint(_reached_destination ? rover.current_loc : _origin, _destination, _reversed);
+                calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, true);
             } else {
                 stop_vehicle();
             }
diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp
index 6b9e792165..5f77e17ec9 100644
--- a/APMrover2/mode_rtl.cpp
+++ b/APMrover2/mode_rtl.cpp
@@ -31,8 +31,8 @@ void ModeRTL::update()
     // determine if we should keep navigating
     if (!_reached_destination || (rover.is_boat() && !near_wp)) {
         // continue driving towards destination
-        calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination);
-        calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
+        calc_steering_to_waypoint(_reached_destination ? rover.current_loc :_origin, _destination, _reversed);
+        calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false);
     } else {
         // we've reached destination so stop
         stop_vehicle();
diff --git a/APMrover2/mode_smart_rtl.cpp b/APMrover2/mode_smart_rtl.cpp
index a60a01dcb8..295385a143 100644
--- a/APMrover2/mode_smart_rtl.cpp
+++ b/APMrover2/mode_smart_rtl.cpp
@@ -64,8 +64,8 @@ void ModeSmartRTL::update()
                 _load_point = true;
             }
             // continue driving towards destination
-            calc_steering_to_waypoint(_origin, _destination);
-            calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
+            calc_steering_to_waypoint(_origin, _destination, _reversed);
+            calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false);
             break;
 
         case SmartRTL_StopAtHome:
@@ -73,8 +73,8 @@ void ModeSmartRTL::update()
             _reached_destination = true;
             if (rover.is_boat()) {
                 // boats attempt to hold position at home
-                calc_steering_to_waypoint(rover.current_loc, _destination);
-                calc_throttle(calc_reduced_speed_for_turn_or_distance(_desired_speed), true, false);
+                calc_steering_to_waypoint(rover.current_loc, _destination, _reversed);
+                calc_throttle(calc_reduced_speed_for_turn_or_distance(_reversed ? -_desired_speed : _desired_speed), true, false);
             } else {
                 // rovers stop
                 stop_vehicle();