From 5d433ddef729ce47a87823c6964a8b2b02c214d0 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 7 Dec 2023 10:39:10 +0100 Subject: [PATCH] TECS: make sure to constrain pitch to current min/max pitch Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index b42a6c2d95..8353d60fdd 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -419,6 +419,7 @@ void TECSControl::_calcPitchControl(float dt, const Input &input, const Specific const float pitch_increment = dt * param.vert_accel_limit / math::max(input.tas, FLT_EPSILON); _pitch_setpoint = constrain(pitch_setpoint, _pitch_setpoint - pitch_increment, _pitch_setpoint + pitch_increment); + _pitch_setpoint = constrain(_pitch_setpoint, param.pitch_min, param.pitch_max); //Debug Output _debug_output.energy_balance_rate_estimate = seb_rate.estimate;