Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_TECS: Protect against too small values of TECS_HDEM_TCONST

This commit is contained in:
Paul Riseborough 2022-11-15 19:11:57 +11:00 committed by Andrew Tridgell
parent ec9445757e
commit d449ed885a
1 changed files with 2 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_TECS/AP_TECS.cpp
View File

@ -285,7 +285,7 @@ const AP_Param::GroupInfo AP_TECS::var_info[] = {
// @Param: HDEM_TCONST
// @DisplayName: Height Demand Time Constant
// @Description: This sets the time constant of the low pass filter that is applied to the height demand input when bit 1 of TECS_OPTIONS is not selected.
// @Range: -1.0 5.0
// @Range: 1.0 5.0
// @Units: s
// @Increment: 0.2
// @User: Advanced
@ -537,7 +537,7 @@ void AP_TECS::_update_height_demand(void)
// Apply a first order lag to height demand and compensate for lag when commencing height
// control after takeoff to prevent plane pushing nose to level before climbing again. Post takeoff
// compensation offset is decayed using the same time constant as the height demand filter.
const float coef = MIN(_DT / (_DT + _hgt_dem_tconst), 1.0f);
const float coef = MIN(_DT / (_DT + MAX(_hgt_dem_tconst, _DT)), 1.0f);
_hgt_rate_dem = (_hgt_dem_rate_ltd - _hgt_dem_lpf) / _hgt_dem_tconst;
_hgt_dem_lpf = _hgt_dem_rate_ltd * coef + (1.0f - coef) * _hgt_dem_lpf;
_post_TO_hgt_offset *= (1.0f - coef);