Plane: fixed TECS state reset in VTOL auto

this fixes a bug where TECS maintains its slow integrator while in a
VTOL hover mode in AUTO or GUIDED.

Among other things this affects PAYLOAD_PLACE and
DO_VTOL_TRANSITION. In those states the height can change while
hovering outside the control of TECS. When TECS regains control in a
fwd transition then can lead to a very large height loss or gain until
the TECS integrator can catch up

ArduPlane/ArduPlane.cpp

@@ -203,7 +203,13 @@ void Plane::ahrs_update()
  */
 void Plane::update_speed_height(void)
 {
-    if (control_mode->does_auto_throttle()) {
+    bool should_run_tecs = control_mode->does_auto_throttle();
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.should_disable_TECS()) {
+        should_run_tecs = false;
+    }
+#endif
+    if (should_run_tecs) {
 	    // Call TECS 50Hz update. Note that we call this regardless of
 	    // throttle suppressed, as this needs to be running for
 	    // takeoff detection
@@ -543,7 +549,14 @@ void Plane::update_alt()
     }
 #endif
 
-    if (control_mode->does_auto_throttle() && !throttle_suppressed) {
+    bool should_run_tecs = control_mode->does_auto_throttle();
+#if HAL_QUADPLANE_ENABLED
+    if (quadplane.should_disable_TECS()) {
+        should_run_tecs = false;
+    }
+#endif
+    
+    if (should_run_tecs && !throttle_suppressed) {
 
         float distance_beyond_land_wp = 0;
         if (flight_stage == AP_FixedWing::FlightStage::LAND &&

ArduPlane/quadplane.cpp

@@ -4550,4 +4550,20 @@ bool QuadPlane::abort_landing(void)
     return true;
 }
 
+/*
+  return true if we should disable TECS in the current flight state
+  this ensures that TECS resets when we change height in a VTOL mode
+ */
+bool QuadPlane::should_disable_TECS() const
+{
+    if (in_vtol_land_descent()) {
+        return true;
+    }
+    if (plane.control_mode == &plane.mode_guided &&
+        plane.auto_state.vtol_loiter) {
+        return true;
+    }
+    return false;
+}
+
 #endif  // HAL_QUADPLANE_ENABLED

ArduPlane/quadplane.h

@@ -180,6 +180,12 @@ public:
      */
     bool in_vtol_land_descent(void) const;
 
+    /*
+      should we disable the TECS controller?
+      only called when in an auto-throttle mode
+     */
+    bool should_disable_TECS() const;
+
 private:
     AP_AHRS &ahrs;