diff --git a/ArduPlane/control_modes.cpp b/ArduPlane/control_modes.cpp
index 5cc01d4fc3..204a0b6ada 100644
--- a/ArduPlane/control_modes.cpp
+++ b/ArduPlane/control_modes.cpp
@@ -44,7 +44,6 @@ void Plane::read_control_switch()
         set_mode((enum FlightMode)(flight_modes[switchPosition].get()));
 
         oldSwitchPosition = switchPosition;
-        prev_WP_loc = current_loc;
     }
 
     if (g.reset_mission_chan != 0 &&
diff --git a/ArduPlane/system.cpp b/ArduPlane/system.cpp
index 0ee00ea9ff..4914d7c11e 100644
--- a/ArduPlane/system.cpp
+++ b/ArduPlane/system.cpp
@@ -363,6 +363,9 @@ void Plane::set_mode(enum FlightMode mode)
     // disable taildrag takeoff on mode change
     auto_state.fbwa_tdrag_takeoff_mode = false;
 
+    // start with previous WP at current location
+    prev_WP_loc = current_loc;
+
     switch(control_mode)
     {
     case INITIALISING: