From a12e40b1d8a061b214238450169e5e8372650a12 Mon Sep 17 00:00:00 2001 From: Thomas Stastny Date: Tue, 12 Jul 2022 14:18:20 +0200 Subject: [PATCH] tecs: convert tas error and soft bounds to percentage of trim airspeed --- src/lib/tecs/TECS.cpp | 13 +++++++++++-- src/lib/tecs/TECS.hpp | 8 ++------ 2 files changed, 13 insertions(+), 8 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index 23aaee81eb..d287eb04f3 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -216,8 +216,17 @@ void TECS::_detect_underspeed() return; } - const float tas_fully_undersped = math::max(_TAS_min - TAS_ERROR_BOUND - TAS_UNDERSPEED_SOFT_BOUND, 0.0f); - const float tas_starting_to_underspeed = math::max(_TAS_min - TAS_ERROR_BOUND, tas_fully_undersped); + // this is the expected (something like standard) deviation from the airspeed setpoint that we allow the airspeed + // to vary in before ramping in underspeed mitigation + const float tas_error_bound = kTASErrorPercentage * _equivalent_airspeed_trim; + + // this is the soft boundary where underspeed mitigation is ramped in + // NOTE: it's currently the same as the error bound, but separated here to indicate these values do not in general + // need to be the same + const float tas_underspeed_soft_bound = kTASErrorPercentage * _equivalent_airspeed_trim; + + const float tas_fully_undersped = math::max(_TAS_min - tas_error_bound - tas_underspeed_soft_bound, 0.0f); + const float tas_starting_to_underspeed = math::max(_TAS_min - tas_error_bound, tas_fully_undersped); _percent_undersped = 1.0f - math::constrain((_tas_state - tas_fully_undersped) / math::max(tas_starting_to_underspeed - tas_fully_undersped, FLT_EPSILON), 0.0f, 1.0f); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index b088499b15..1f8a58b955 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -219,12 +219,8 @@ public: private: - // [m/s] bound on expected (safe) true airspeed tracking errors, including TAS = TAS_min - TAS_ERROR_BOUND - static constexpr float TAS_ERROR_BOUND = 2.0f; - - // [m/s] true airspeed soft boundary region below the accepted TAS error region (below TAS_min - TAS_ERROR_BOUND) - // underspeed mitigation measures are ramped in from zero to full within this region - static constexpr float TAS_UNDERSPEED_SOFT_BOUND = 1.5f; + // [0,1] percentage of true airspeed trim corresponding to expected (safe) true airspeed tracking errors + static constexpr float kTASErrorPercentage = 0.15; static constexpr float _jerk_max = 1000.0f;