From d449ed885a9935af50d9a12f2345734a8c1c7d01 Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Tue, 15 Nov 2022 19:11:57 +1100 Subject: [PATCH] AP_TECS: Protect against too small values of TECS_HDEM_TCONST --- libraries/AP_TECS/AP_TECS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 6217565511..ef3c5ce710 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -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);