From 97cc1e25dd62bcf6bf233c90b060925b3057586e Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Fri, 30 Oct 2020 07:23:53 +0300 Subject: [PATCH] TECS: hande pitch and throttle integrator saturation consistently - don't decay integrator during saturation but prevent it from being learned into the direction which causes more actuator saturation Signed-off-by: RomanBapst --- src/lib/tecs/TECS.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 12d0a6531d..1da55d775f 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -429,12 +429,11 @@ void TECS::_update_pitch_setpoint() float pitch_integ_input = _SEB_rate_error * _integrator_gain_pitch; // Prevent the integrator changing in a direction that will increase pitch demand saturation - // Decay the integrator using a 1 second time constant if the pitch demand from the previous time step is saturated if (_pitch_setpoint_unc > _pitch_setpoint_max) { - pitch_integ_input = min(pitch_integ_input, (_pitch_setpoint_max - _pitch_setpoint_unc) * climb_angle_to_SEB_rate); + pitch_integ_input = min(pitch_integ_input, 0.f); } else if (_pitch_setpoint_unc < _pitch_setpoint_min) { - pitch_integ_input = max(pitch_integ_input, (_pitch_setpoint_min - _pitch_setpoint_unc) * climb_angle_to_SEB_rate); + pitch_integ_input = max(pitch_integ_input, 0.f); } // Update the pitch integrator state.