Skip to content

Commit 75fb2c8

Browse files
committed
Plane: Use stall speed for roll stall prevention
The new AIRSPEED_STALL parameter wasn't being used for the roll stall prevention, and no adjustment for bank angle was being made. This led to a slight under-limiting of bank angle. Fixed both issues by using the stall airspeed from the TECS, to avoid repeating the same logic, and adjusted the description of the AIRSPEED_STALL parameter accordingly. This improves consistency as it uses AIRSPEED_STALL for all stall prevention calculations where it applies, and sets up AIRSPEED_STALL to become a key parameter in further stall prevention improvements.
1 parent dc0bbb0 commit 75fb2c8

File tree

2 files changed

+4
-3
lines changed

2 files changed

+4
-3
lines changed

ArduPlane/Attitude.cpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -659,7 +659,8 @@ void Plane::update_load_factor(void)
659659
}
660660
#endif
661661

662-
float max_load_factor = sq(smoothed_airspeed / MAX(aparm.airspeed_min, 1));
662+
float max_load_factor =
663+
sq(smoothed_airspeed / MAX(TECS_controller.get_stall_airspeed(), 1));
663664

664665
if (max_load_factor <= 1) {
665666
// our airspeed is below the minimum airspeed. Limit roll to
@@ -671,7 +672,7 @@ void Plane::update_load_factor(void)
671672
// load limit. Limit our roll to a bank angle that will keep
672673
// the load within what the airframe can handle. We always
673674
// allow at least 25 degrees of roll however, to ensure the
674-
// aircraft can be manoeuvred with a bad airspeed estimate. At
675+
// aircraft can be manoeuvered with a bad airspeed estimate. At
675676
// 25 degrees the load factor is 1.1 (10%)
676677
int32_t roll_limit = degrees(acosf(1.0f / max_load_factor))*100;
677678
if (roll_limit < 2500) {

ArduPlane/Parameters.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -323,7 +323,7 @@ const AP_Param::Info Plane::var_info[] = {
323323

324324
// @Param: AIRSPEED_STALL
325325
// @DisplayName: Stall airspeed
326-
// @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.
326+
// @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.
327327
// @Units: m/s
328328
// @Range: 5 75
329329
// @User: Standard

0 commit comments

Comments
 (0)