From 7926107328ef007637c4857488f5cf88702ae0ec Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 11 Dec 2023 11:59:53 +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 7b462684e8..cd765b71b5 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -412,6 +412,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;