Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 06:28:27 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Reverted pitch limitation order

Browse Source
This commit is contained in:
George Zogopoulos 2024-09-26 16:27:27 +02:00 committed by Andrew Tridgell
parent 121adf5f6d
commit 68003a5eb4
1 changed files with 3 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
libraries/AP_TECS/AP_TECS.cpp
View File

@ -1053,6 +1053,9 @@ void AP_TECS::_update_pitch(void)
_pitch_dem_unc += (_TAS_dem_adj - _pitch_ff_v0) * _pitch_ff_k; _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 // Rate limit the pitch demand to comply with specified vertical
// acceleration limit // acceleration limit
float ptchRateIncr = _DT * _vertAccLim / _TAS_state; float ptchRateIncr = _DT * _vertAccLim / _TAS_state;
@ -1063,9 +1066,6 @@ void AP_TECS::_update_pitch(void)
_pitch_dem = _last_pitch_dem - ptchRateIncr; _pitch_dem = _last_pitch_dem - ptchRateIncr;
} }
// Constrain pitch demand
_pitch_dem = constrain_float(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
_last_pitch_dem = _pitch_dem; _last_pitch_dem = _pitch_dem;
#if HAL_LOGGING_ENABLED #if HAL_LOGGING_ENABLED