From 1782f9cd3e8460c574ff86e64bc413c08ed4b1fa Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 12 Jul 2022 13:56:30 +0200 Subject: [PATCH] tecs: zero guard ste rates and airspeed setpoint rates --- src/lib/tecs/TECS.cpp | 9 ++++----- src/lib/tecs/TECS.hpp | 4 ++-- 2 files changed, 6 insertions(+), 7 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index bfd3b06f78..23aaee81eb 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -168,9 +168,8 @@ void TECS::_update_speed_setpoint() // Calculate limits for the demanded rate of change of speed based on physical performance limits // with a 50% margin to allow the total energy controller to correct for errors. - // NOTE: at zero airspeed, the true airspeed rate setpoint is unbounded - const float max_tas_rate_sp = 0.5f * _STE_rate_max / _tas_state; - const float min_tas_rate_sp = 0.5f * _STE_rate_min / _tas_state; + const float max_tas_rate_sp = 0.5f * _STE_rate_max / math::max(_tas_state, FLT_EPSILON); + const float min_tas_rate_sp = 0.5f * _STE_rate_min / math::max(_tas_state, FLT_EPSILON); _TAS_setpoint_adj = constrain(_TAS_setpoint, _TAS_min, _TAS_max); @@ -548,10 +547,10 @@ void TECS::_initialize_states(float pitch, float throttle_trim, float baro_altit void TECS::_update_STE_rate_lim() { // Calculate the specific total energy upper rate limits from the max throttle climb rate - _STE_rate_max = _max_climb_rate * CONSTANTS_ONE_G; + _STE_rate_max = math::max(_max_climb_rate, FLT_EPSILON) * CONSTANTS_ONE_G; // Calculate the specific total energy lower rate limits from the min throttle sink rate - _STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G; + _STE_rate_min = - math::max(_min_sink_rate, FLT_EPSILON) * CONSTANTS_ONE_G; } void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 6e37dc2141..b088499b15 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -291,8 +291,8 @@ private: // vehicle physical limits float _pitch_setpoint_unc{0.0f}; ///< pitch demand before limiting (rad) - float _STE_rate_max{0.0f}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3) - float _STE_rate_min{0.0f}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3) + float _STE_rate_max{FLT_EPSILON}; ///< specific total energy rate upper limit achieved when throttle is at _throttle_setpoint_max (m**2/sec**3) + float _STE_rate_min{-FLT_EPSILON}; ///< specific total energy rate lower limit acheived when throttle is at _throttle_setpoint_min (m**2/sec**3) float _throttle_setpoint_max{0.0f}; ///< normalised throttle upper limit float _throttle_setpoint_min{0.0f}; ///< normalised throttle lower limit float _throttle_trim{0.0f}; ///< throttle required to fly level at _EAS_setpoint, compensated for air density and vehicle weight