Plane: prevent mode switch changes changing WP tracking

this fixes a bug where a mode switch change during an AUTO mission
which does not change the flight mode would cause cross tracking to be
reset, so the plane will not correctly follow the desired track

Many thanks to Michael Du Breuil for the log that showed this bug

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 &&

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: