Plane: fixed QRTL change when coming from loiter

the nav controller can think we have already reached the loiter target
if we were last in a LOITER when we switch to RTL. In that case it
would switch to QRTL immediately

found by Leonard (thanks!)

ArduPlane/ArduPlane.cpp

@@ -801,7 +801,8 @@ void Plane::update_navigation()
             
     case RTL:
         if (quadplane.available() && quadplane.rtl_mode == 1 &&
-            nav_controller->reached_loiter_target()) {
+            nav_controller->reached_loiter_target() &&
+            AP_HAL::millis() - last_mode_change_ms > 1000) {
             set_mode(QRTL, MODE_REASON_UNKNOWN);
             break;
         } else if (g.rtl_autoland == 1 &&

ArduPlane/Plane.h

@@ -294,6 +294,9 @@ private:
     enum FlightMode previous_mode = INITIALISING;
     mode_reason_t previous_mode_reason = MODE_REASON_UNKNOWN;
 
+    // time of last mode change
+    uint32_t last_mode_change_ms;
+
     // Used to maintain the state of the previous control switch position
     // This is set to 254 when we need to re-read the switch
     uint8_t oldSwitchPosition = 254;

ArduPlane/system.cpp

@@ -380,6 +380,9 @@ void Plane::set_mode(enum FlightMode mode, mode_reason_t reason)
     // new mode means new loiter
     loiter.start_time_ms = 0;
 
+    // record time of mode change
+    last_mode_change_ms = AP_HAL::millis();
+    
     // assume non-VTOL mode
     auto_state.vtol_mode = false;
     auto_state.vtol_loiter = false;