From e3ac2d1a3c08510d249ec441138385247ff919ba Mon Sep 17 00:00:00 2001 From: abaghiyan Date: Sat, 5 Aug 2023 00:35:15 +0400 Subject: [PATCH 1/4] Plane: Fixed formula for load factor In general when there the pitch and roll exist the formula for the aerodynamic load factor should be as follows: aerodynamic_load_factor = cosf(radians(_ahrs.pitch))/cosf(radians(demanded_roll)); which in case of turns (in the horizontal plane) becomes aerodynamic_load_factor = 1.0f/cosf(radians(demanded_roll)); formulas can be obtained from equations of balanced spiral: liftForce * cos(roll) = gravityForce * cos(pitch); liftForce * sin(roll) = gravityForce * lateralAcceleration / gravityAcceleration; // as mass = gravityForce/gravityAcceleration see issue #24320 [#24320]. 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); These changes also require changes in ardupilot/libraries/AP_TECS/AP_TECS.cpp Line 418 (according to the comments by Peter Hall): _TASmin *= _load_factor; should be changed to _TASmin *= safe_sqrt(_load_factor); See details here: #24320 --- ArduPlane/Attitude.cpp | 11 +++++++---- ArduPlane/Plane.h | 2 +- 2 files changed, 8 insertions(+), 5 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 17cc78bc173975..2ba65b409cc402 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -633,7 +633,9 @@ void Plane::update_load_factor(void) // limit to 85 degrees to prevent numerical errors demanded_roll = 85; } - aerodynamic_load_factor = 1.0f / safe_sqrt(cosf(radians(demanded_roll))); + + // loadFactor = liftForce / gravityForce, where gravityForce = liftForce * cos(roll) on balanced horizontal turn + aerodynamic_load_factor = 1.0f / cosf(radians(demanded_roll)); #if HAL_QUADPLANE_ENABLED if (quadplane.available() && quadplane.transition->set_FW_roll_limit(roll_limit_cd)) { @@ -657,7 +659,8 @@ void Plane::update_load_factor(void) } #endif - float max_load_factor = smoothed_airspeed / MAX(aparm.airspeed_min, 1); + float max_load_factor = sq(smoothed_airspeed / MAX(aparm.airspeed_min, 1)); + if (max_load_factor <= 1) { // our airspeed is below the minimum airspeed. Limit roll to // 25 degrees @@ -670,11 +673,11 @@ void Plane::update_load_factor(void) // allow at least 25 degrees of roll however, to ensure the // aircraft can be manoeuvred with a bad airspeed estimate. At // 25 degrees the load factor is 1.1 (10%) - int32_t roll_limit = degrees(acosf(sq(1.0f / max_load_factor)))*100; + int32_t roll_limit = degrees(acosf(1.0f / max_load_factor))*100; if (roll_limit < 2500) { roll_limit = 2500; } nav_roll_cd = constrain_int32(nav_roll_cd, -roll_limit, roll_limit); roll_limit_cd = MIN(roll_limit_cd, roll_limit); - } + } } diff --git a/ArduPlane/Plane.h b/ArduPlane/Plane.h index 1dda8aa741f40e..91e1d56bdcf941 100644 --- a/ArduPlane/Plane.h +++ b/ArduPlane/Plane.h @@ -672,7 +672,7 @@ class Plane : public AP_Vehicle { int32_t nav_pitch_cd; // the aerodynamic load factor. This is calculated from the demanded - // roll before the roll is clipped, using 1/sqrt(cos(nav_roll)) + // roll before the roll is clipped, using 1/cos(nav_roll) float aerodynamic_load_factor = 1.0f; // a smoothed airspeed estimate, used for limiting roll angle From 089f5778df54e3221d6b78f61988e5adfbb72936 Mon Sep 17 00:00:00 2001 From: abaghiyan Date: Sat, 5 Aug 2023 00:37:31 +0400 Subject: [PATCH 2/4] 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 b0cbe34087e29f..f6f8811efbaf26 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); } } From d5ccb9c6c8b3b187b15d875026a5a01b568d3c30 Mon Sep 17 00:00:00 2001 From: rubenp02 Date: Mon, 20 Jan 2025 16:37:36 +0100 Subject: [PATCH 3/4] TECS: Add getters for pratical stall airspeed --- libraries/AP_TECS/AP_TECS.h | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/libraries/AP_TECS/AP_TECS.h b/libraries/AP_TECS/AP_TECS.h index 484177ed920d9c..4fbaf2cdf0307c 100644 --- a/libraries/AP_TECS/AP_TECS.h +++ b/libraries/AP_TECS/AP_TECS.h @@ -77,6 +77,20 @@ class AP_TECS { return _TAS_dem_adj / _ahrs.get_EAS2TAS(); } + // Return stall airspeed, adjusted for bank angle. This is based on + // AIRSPEED_MIN as a very conservative estimate if AIRSPEED_STALL is not + // set. + float get_stall_airspeed(void) const { + return _TASmin / _ahrs.get_EAS2TAS(); + } + + // Return stall airspeed on level flight. This is based on AIRSPEED_MIN as a + // very conservative estimate if AIRSPEED_STALL is not set. + float get_stall_airspeed_1g(void) const { + return is_positive(aparm.airspeed_stall) ? aparm.airspeed_stall + : aparm.airspeed_min; + } + // return maximum climb rate float get_max_climbrate(void) const { return _maxClimbRate; From f1a99eca434ee91ffc479c42eeccdfc2c10b5655 Mon Sep 17 00:00:00 2001 From: rubenp02 Date: Thu, 16 Jan 2025 15:33:46 +0100 Subject: [PATCH 4/4] Plane: Use stall speed for roll stall prevention --- ArduPlane/Attitude.cpp | 5 +++-- ArduPlane/Parameters.cpp | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/ArduPlane/Attitude.cpp b/ArduPlane/Attitude.cpp index 2ba65b409cc402..9a6e5cfeef5775 100644 --- a/ArduPlane/Attitude.cpp +++ b/ArduPlane/Attitude.cpp @@ -659,7 +659,8 @@ void Plane::update_load_factor(void) } #endif - float max_load_factor = sq(smoothed_airspeed / MAX(aparm.airspeed_min, 1)); + float max_load_factor = + sq(smoothed_airspeed / MAX(TECS_controller.get_stall_airspeed_1g(), 1)); if (max_load_factor <= 1) { // our airspeed is below the minimum airspeed. Limit roll to @@ -671,7 +672,7 @@ void Plane::update_load_factor(void) // load limit. Limit our roll to a bank angle that will keep // the load within what the airframe can handle. We always // allow at least 25 degrees of roll however, to ensure the - // aircraft can be manoeuvred with a bad airspeed estimate. At + // aircraft can be manoeuvered with a bad airspeed estimate. At // 25 degrees the load factor is 1.1 (10%) int32_t roll_limit = degrees(acosf(1.0f / max_load_factor))*100; if (roll_limit < 2500) { diff --git a/ArduPlane/Parameters.cpp b/ArduPlane/Parameters.cpp index 061d3cb5ac6598..f6fb4a1a5c4049 100644 --- a/ArduPlane/Parameters.cpp +++ b/ArduPlane/Parameters.cpp @@ -323,7 +323,7 @@ const AP_Param::Info Plane::var_info[] = { // @Param: AIRSPEED_STALL // @DisplayName: Stall airspeed - // @Description: If stall prevention is enabled this speed is used to calculate the minimum airspeed while banking. If this is set to 0 then the stall speed is assumed to be the minimum airspeed speed. Typically set slightly higher then true stall speed. Value is as an indicated (calibrated/apparent) airspeed. + // @Description: If stall prevention is enabled this speed is used for its calculations. If set to 0 then AIRSPEED_MIN is used instead. Typically set slightly higher than the true stall speed. Value is as an indicated (calibrated/apparent) airspeed. // @Units: m/s // @Range: 5 75 // @User: Standard