AP_TECS: Constrain pitch to limits after rate constraint

ArduPilot · Aug 13, 2024 · e9aa49b · e9aa49b
1 parent 65dc003
commit e9aa49b
Showing 1 changed file with 3 additions and 3 deletions.
diff --git 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