From 5c4c2c240c5c39bb8743e480e817c798d8cc17f5 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Sun, 25 Oct 2020 08:22:54 +0300 Subject: [PATCH] tecs: dot not clip throttle integrator during saturation but only allow it to propagate into a direction which does not saturate Signed-off-by: RomanBapst --- src/lib/tecs/TECS.cpp | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index ea509bd3bd..f059ccb805 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -329,15 +329,24 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix:: throttle_setpoint = constrain(throttle_setpoint, _throttle_setpoint_min, _throttle_setpoint_max); if (_integrator_gain_throttle > 0.0f) { - // Calculate throttle integrator state upper and lower limits with allowance for - // 10% throttle saturation to accommodate noise on the demand. - float integ_state_max = _throttle_setpoint_max - throttle_setpoint + 0.1f; - float integ_state_min = _throttle_setpoint_min - throttle_setpoint - 0.1f; - // Calculate a throttle demand from the integrated total energy error + float integ_state_max = _throttle_setpoint_max - throttle_setpoint; + float integ_state_min = _throttle_setpoint_min - throttle_setpoint; + + float throttle_integ_input = (_STE_rate_error * _integrator_gain_throttle) * _dt * + STE_rate_to_throttle; + + // only allow integrator propagation into direction which unsaturates throttle + if (_throttle_integ_state > integ_state_max) { + throttle_integ_input = math::min(0.f, throttle_integ_input); + + } else if (_throttle_integ_state < integ_state_min) { + throttle_integ_input = math::max(0.f, throttle_integ_input); + } + + // Calculate a throttle demand from the integrated total energy rate error // This will be added to the total throttle demand to compensate for steady state errors - _throttle_integ_state = _throttle_integ_state + (_STE_rate_error * _integrator_gain_throttle) * _dt * - STE_rate_to_throttle; + _throttle_integ_state = _throttle_integ_state + throttle_integ_input; if (_climbout_mode_active) { // During climbout, set the integrator to maximum throttle to prevent transient throttle drop