From b57d3c6d746dc0a68be49310a25d51615d913ad2 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 7 Jul 2022 13:00:23 +0300 Subject: [PATCH] TECS: set true airspeed rate setpoint to zero if airspeed is not enabled Signed-off-by: RomanBapst --- src/lib/tecs/TECS.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index e33250e226..019f667d31 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -170,7 +170,13 @@ void TECS::_update_speed_setpoint() _TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max); // calculate the demanded true airspeed rate of change based on first order response of true airspeed error - _TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax); + // if airspeed measurement is not enabled then always set the rate setpoint to zero in order to avoid constant rate setpoints + if (airspeed_sensor_enabled()) { + _TAS_rate_setpoint = constrain((_TAS_setpoint_adj - _tas_state) * _airspeed_error_gain, velRateMin, velRateMax); + + } else { + _TAS_rate_setpoint = 0.0f; + } }