From 7f5973e91f8a00f4eaa499f7ac3783758abe6809 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 21 Aug 2025 17:07:25 +0200 Subject: [PATCH] TECS: fix: reset altitude reference model if we're using the height rate interface Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 5ff73eb83c..7b3221d682 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -745,8 +745,8 @@ void TECS::update(float pitch, float altitude, float hgt_setpoint, float EAS_set _airspeed_filter.update(dt, airspeed_input, _airspeed_filter_param, _control_flag.airspeed_enabled); // Update Reference model submodule - if (1.f - _fast_descend < FLT_EPSILON) { - // Reset the altitude reference model, while we are in fast descend. + if (1.f - _fast_descend < FLT_EPSILON || !PX4_ISFINITE(hgt_rate_sp)) { + // Reset the altitude reference model while we are in fast descend or in height rate control mode const TECSAltitudeReferenceModel::AltitudeReferenceState init_state{ .alt = altitude, .alt_rate = hgt_rate};