diff --git a/APMrover2/commands_logic.cpp b/APMrover2/commands_logic.cpp
index 741a8cc44d..10a9f0b37c 100644
--- a/APMrover2/commands_logic.cpp
+++ b/APMrover2/commands_logic.cpp
@@ -215,9 +215,7 @@ bool Rover::verify_command(const AP_Mission::Mission_Command& cmd)
 
 void Rover::do_RTL(void)
 {
-    prev_WP = current_loc;
-    control_mode = &mode_rtl;
-    next_WP = home;
+    set_mode(mode_rtl);
 }
 
 void Rover::do_nav_wp(const AP_Mission::Mission_Command& cmd)
diff --git a/APMrover2/mode_rtl.cpp b/APMrover2/mode_rtl.cpp
index ac92ecb8fc..d0f94c78c8 100644
--- a/APMrover2/mode_rtl.cpp
+++ b/APMrover2/mode_rtl.cpp
@@ -3,7 +3,8 @@
 
 bool ModeRTL::_enter()
 {
-    rover.do_RTL();
+    rover.prev_WP = rover.current_loc;
+    rover.next_WP = rover.home;
     g2.motors.slew_limit_throttle(true);
     return true;
 }