From 19b50442235e292aeedac39f90b8e3d46c49f118 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Fri, 29 Oct 2021 07:43:15 +1100
Subject: [PATCH] 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 | 16 +++++++++++++++-
 ArduPlane/quadplane.h   |  1 +
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 2e5e52966d..a826f39c70 100644
--- a/ArduPlane/quadplane.cpp
+++ b/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();
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 1283bce7bb..e8aa1053d6 100644
--- a/ArduPlane/quadplane.h
+++ b/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;