From 0232fa74560c6ebb48c3e32da3b18f2cb33df1b3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 1 Jun 2021 18:10:53 +1000
Subject: [PATCH] Plane: cope with fwd thrust loss in Q approach

if throttle is saturated and descending and low airspeed then declare
thrust loss
---
 ArduPlane/quadplane.cpp | 19 +++++++++++++++++++
 ArduPlane/quadplane.h   |  1 +
 2 files changed, 20 insertions(+)

diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp
index 637c5338e1..7f0a621623 100644
--- a/ArduPlane/quadplane.cpp
+++ b/ArduPlane/quadplane.cpp
@@ -2568,6 +2568,7 @@ void QuadPlane::poscontrol_init_approach(void)
                         poscontrol.start_closing_vel,
                         poscontrol.start_dist);
         poscontrol.set_state(QPOS_APPROACH);
+        poscontrol.thrust_loss_start_ms = 0;
     }
 }
 
@@ -2675,6 +2676,24 @@ void QuadPlane::vtol_position_controller(void)
             vel_forward.last_ms = AP_HAL::millis();
         }
 
+        bool throttle_saturated = SRV_Channels::get_output_scaled(SRV_Channel::k_throttle) >= plane.aparm.throttle_max;
+        if (throttle_saturated &&
+            motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED &&
+            plane.auto_state.sink_rate > 0.2 && aspeed < aspeed_threshold+4) {
+            const uint32_t now = AP_HAL::millis();
+            if (poscontrol.thrust_loss_start_ms == 0) {
+                poscontrol.thrust_loss_start_ms = now;
+            }
+            if (now - poscontrol.thrust_loss_start_ms > 5000) {
+                gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 thrust loss as=%.1f at=%.1f",
+                                aspeed, aspeed_threshold);
+                poscontrol.set_state(QPOS_POSITION1);
+            }
+        } else {
+            poscontrol.thrust_loss_start_ms = 0;
+        }
+
+
         // handle loss of forward thrust in approach
         if (poscontrol.get_state() == QPOS_APPROACH && aspeed < aspeed_threshold &&
             motors->get_desired_spool_state() < AP_Motors::DesiredSpoolState::THROTTLE_UNLIMITED) {
diff --git a/ArduPlane/quadplane.h b/ArduPlane/quadplane.h
index 383aefbe9b..1f85326281 100644
--- a/ArduPlane/quadplane.h
+++ b/ArduPlane/quadplane.h
@@ -489,6 +489,7 @@ private:
         bool pilot_correction_done;
         float start_closing_vel;
         float start_dist;
+        uint32_t thrust_loss_start_ms;
     private:
         uint32_t last_state_change_ms;
         enum position_control_state state;