diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 71c7f5f07e..59e75cda92 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -805,8 +805,7 @@ void TECS::_setFastDescend(const float alt_setpoint, const float alt) static_cast(FAST_DESCEND_RAMP_UP_TIME)), 0.f, 1.f); } else if (PX4_ISFINITE(alt_setpoint) && (_fast_descend > FLT_EPSILON) && (_fast_descend_alt_err > FLT_EPSILON)) { - // Were in fast descend, scale it down. up until 5m above target altitude - _fast_descend = constrain((alt - alt_setpoint - 5.f) / _fast_descend_alt_err, 0.f, 1.f); + _fast_descend = constrain((alt - alt_setpoint - FAST_DESCEND_RAMP_DOWN_MARGIN) / _fast_descend_alt_err, 0.f, 1.f); _enabled_fast_descend_timestamp = 0U; } else { diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index a19c4723ca..4e0e6aa752 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -704,6 +704,7 @@ private: static constexpr float DT_MIN = 0.001f; ///< minimum allowed value of _dt (sec) static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a filter state reset is performed (sec) static constexpr hrt_abstime FAST_DESCEND_RAMP_UP_TIME = 2_s; ///< Ramp up time until fast descend is fully engaged + static constexpr float FAST_DESCEND_RAMP_DOWN_MARGIN = 5.f; ///< Altitude above setpoint [m] at which fast-descend ramp-out reaches zero DebugOutput _debug_status{};