From 7f0869c9d7909cfdc6720b0fd230aae75d430770 Mon Sep 17 00:00:00 2001 From: Konrad Date: Fri, 12 Jul 2024 16:57:29 +0200 Subject: [PATCH] tecs: make sure that in fast descend the throttle is not completely shut down while still climbing --- src/lib/tecs/TECS.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 0c264706831d..4a6d3593e894 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -524,8 +524,13 @@ void TECSControl::_calcThrottleControl(float dt, const SpecificEnergyRates &spec float throttle_setpoint{param.throttle_min}; if (1.f - param.fast_descend < FLT_EPSILON) { - // During fast descend, we control airspeed over the pitch control loop and give minimal thrust. - throttle_setpoint = param.throttle_min; + // During fast descend, we control airspeed over the pitch control loop. Give minimal thrust as soon as we are descending + if (specific_energy_rates.spe_rate.estimate > 0) { // We have a positive altitude rate and are stil climbing + throttle_setpoint = param.throttle_trim; // Do not cut off throttle yet + + } else { + throttle_setpoint = param.throttle_min; + } } else { _calcThrottleControlUpdate(dt, limit, ste_rate, param, flag);