From 4c48f09e77f2ef3e08dfeab31630e42adabd4821 Mon Sep 17 00:00:00 2001 From: George Zogopoulos Date: Thu, 26 Sep 2024 16:27:27 +0200 Subject: [PATCH] AP_TECS: Reverted pitch limitation order --- 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 94bd4bffc77d25..f25cf5294e6f93 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -1053,6 +1053,9 @@ 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; @@ -1063,9 +1066,6 @@ 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