From 245ded2f2dd0c7dee0024a159145c357d72acd17 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Thu, 20 May 2021 12:49:44 +1000 Subject: [PATCH] Plane: fixed handling of loss of fwd thrust in QRTL --- ArduPlane/quadplane.cpp | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index a355322ed1..d4fc0d7dc1 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -2601,7 +2601,7 @@ void QuadPlane::vtol_position_controller(void) } // speed for crossover to POSITION1 controller - const float aspeed_threshold = MAX(plane.aparm.airspeed_min, assist_speed); + const float aspeed_threshold = MAX(plane.aparm.airspeed_min-2, assist_speed); // run fixed wing navigation plane.nav_controller->update_waypoint(plane.current_loc, loc); @@ -2675,6 +2675,15 @@ void QuadPlane::vtol_position_controller(void) vel_forward.last_ms = AP_HAL::millis(); } + // 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) { + gcs().send_text(MAV_SEVERITY_INFO,"VTOL pos1 low speed as=%.1f at=%.1f", + aspeed, aspeed_threshold); + poscontrol.set_state(QPOS_POSITION1); + } + + if (poscontrol.get_state() == QPOS_APPROACH) { poscontrol_init_approach(); }