From 9893017f3c905915b4f036a29bb2243d33c09ab1 Mon Sep 17 00:00:00 2001 From: abaghiyan Date: Sat, 5 Aug 2023 00:37:31 +0400 Subject: [PATCH] AP_TECS: Corrected formula for _TASmin according to fix in formula for the load factor To connect loadFactor to airspeed we can use formula of balancing between lift force and gravity force: liftForce = loadFactor * gravityForce; on the other hand lift force can be expressed as liftForce = 0.5 * lifCoefficient * airDensity * sq(airspeed) * referenceArea; minimum airseepd is at loadFactor = 1 and lift force only balances the gravit force, so gravity force (which is same as lift force at minimum airspeed) with minimum airspeed can be expressed as gravityForce = 0.5 * lifCoefficient * airDensity * sq(airspeed_min) * referenceArea; substituting gravit force in previous formula gives us 0.5 * lifCoefficient * airDensity * sq(airspeed) * referenceArea = loadFactor * 0.5 * lifCoefficient * airDensity * sq(airspeed_min) * referenceArea; from where we get: loadFactor = sq(airspeed / airspeed_min); and_TASmin should be _TASmin *= safe_sqrt(_load_factor); --- libraries/AP_TECS/AP_TECS.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/libraries/AP_TECS/AP_TECS.cpp b/libraries/AP_TECS/AP_TECS.cpp index 5f23480dde..db9c610d4e 100644 --- a/libraries/AP_TECS/AP_TECS.cpp +++ b/libraries/AP_TECS/AP_TECS.cpp @@ -415,9 +415,9 @@ void AP_TECS::_update_speed(float DT) // when stall prevention is active we raise the minimum // airspeed based on aerodynamic load factor if (is_positive(aparm.airspeed_stall)) { - _TASmin = MAX(_TASmin, aparm.airspeed_stall*EAS2TAS*_load_factor); + _TASmin = MAX(_TASmin, aparm.airspeed_stall*EAS2TAS*sqrtf(_load_factor)); } else { - _TASmin *= _load_factor; + _TASmin *= sqrtf(_load_factor); } }