Plane: fixed VTOL state error when using Q_ENABLE=2

this ensures the poscontrol state is reset if it has not been
initialised before a VTOL loiter operation

thanks to Pete Hall for finding this

ArduPlane/quadplane.cpp

@@ -2135,6 +2135,10 @@ void QuadPlane::PosControlState::set_state(enum position_control_state s)
     }
     state = s;
     last_state_change_ms = AP_HAL::millis();
+
+    // we consider setting the state to be equivalent to running to
+    // prevent code from overriding the state as stale
+    last_run_ms = last_state_change_ms;
 }
 
 /*
@@ -2155,6 +2159,10 @@ void QuadPlane::vtol_position_controller(void)
     // target speed when we reach position2 threshold
     const float position2_target_speed = 2.0;
 
+    if (hal.util->get_soft_armed()) {
+        poscontrol.last_run_ms = now_ms;
+    }
+
     // horizontal position control
     switch (poscontrol.get_state()) {
 
@@ -2735,8 +2743,14 @@ void QuadPlane::control_auto(void)
     case MAV_CMD_NAV_LOITER_UNLIM:
     case MAV_CMD_NAV_LOITER_TIME:
     case MAV_CMD_NAV_LOITER_TURNS:
-    case MAV_CMD_NAV_LOITER_TO_ALT:
+    case MAV_CMD_NAV_LOITER_TO_ALT: {
+        const uint32_t now = AP_HAL::millis();
+        if (now - poscontrol.last_run_ms > 100) {
+            // ensure that poscontrol is reset
+            poscontrol.set_state(QPOS_POSITION1);
+        }
         vtol_position_controller();
+    }
         break;
     default:
         waypoint_controller();

ArduPlane/quadplane.h

@@ -445,6 +445,7 @@ private:
         uint32_t thrust_loss_start_ms;
         uint32_t last_log_ms;
         bool reached_wp_speed;
+        uint32_t last_run_ms;
     private:
         uint32_t last_state_change_ms;
         enum position_control_state state;