From 550bbd9051376555808345a0dd3185df0c88d5f0 Mon Sep 17 00:00:00 2001 From: Roman Bapst Date: Thu, 27 Mar 2025 16:22:40 +0300 Subject: [PATCH] FW rate controller: Don't constrain airspeed for scaling to maximum airspeed (#24622) * don't constrain airspeed for scaling to maximum airspeed Signed-off-by: RomanBapst * fix max function Signed-off-by: RomanBapst * remove hardcoded max Signed-off-by: RomanBapst --------- Signed-off-by: RomanBapst --- src/modules/fw_rate_control/FixedwingRateControl.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_rate_control/FixedwingRateControl.cpp b/src/modules/fw_rate_control/FixedwingRateControl.cpp index 9841ea8ca6..c321b4edcd 100644 --- a/src/modules/fw_rate_control/FixedwingRateControl.cpp +++ b/src/modules/fw_rate_control/FixedwingRateControl.cpp @@ -179,10 +179,16 @@ float FixedwingRateControl::get_airspeed_and_update_scaling() * * Forcing the scaling to this value allows reasonable handheld tests. */ - const float airspeed_constrained = constrain(constrain(airspeed, _param_fw_airspd_stall.get(), - _param_fw_airspd_max.get()), 0.1f, 1000.0f); - _airspeed_scaling = (_param_fw_arsp_scale_en.get()) ? (_param_fw_airspd_trim.get() / airspeed_constrained) : 1.0f; + + if (_param_fw_arsp_scale_en.get()) { + const float min_airspeed = math::max(_param_fw_airspd_stall.get(), 0.1f); + const float airspeed_constrained = math::max(airspeed, min_airspeed); + _airspeed_scaling = _param_fw_airspd_trim.get() / airspeed_constrained; + + } else { + _airspeed_scaling = 1.0f; + } return airspeed; }