From e9aa49b2f4402ee59e6a09f24f1bb44789d79a82 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Tue, 13 Aug 2024 16:47:45 +0200 Subject: [PATCH] AP_TECS: Constrain pitch to limits after rate constraint --- libraries/AP_TECS/AP_TECS.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index e3843a02aeabcf..6dd211463228c6 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -1054,9 +1054,6 @@ void AP_TECS::_update_pitch(void) _pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k; } - // Constrain pitch demand - _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); - // Rate limit the pitch demand to comply with specified vertical // acceleration limit float ptchRateIncr = _DT * _vertAccLim / _TAS_state; @@ -1067,6 +1064,9 @@ void AP_TECS::_update_pitch(void) _pitch_dem = _last_pitch_dem - ptchRateIncr; } + // Constrain pitch demand + _pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf); + _last_pitch_dem = _pitch_dem; #if HAL_LOGGING_ENABLED