From 83280dcfec1fc8e5e350c8d76ddaa8525ace8350 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 10 Apr 2025 11:28:37 +0200 Subject: [PATCH] TECS: protect against NAN in pitch integrator by limiting tas input to min tas Signed-off-by: Silvan Fuhrer --- src/lib/tecs/TECS.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index e519246099..5ff73eb83c 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -469,7 +469,7 @@ void TECSControl::_calcPitchControlUpdate(float dt, const Input &input, const Co if (param.integrator_gain_pitch > FLT_EPSILON) { // Calculate derivative from change in climb angle to rate of change of specific energy balance - const float climb_angle_to_SEB_rate = input.tas * CONSTANTS_ONE_G; + const float climb_angle_to_SEB_rate = max(input.tas, param.tas_min) * CONSTANTS_ONE_G; // Calculate pitch integrator input term float pitch_integ_input = _getControlError(seb_rate) * param.integrator_gain_pitch / climb_angle_to_SEB_rate;