Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Plane: Attitude stall prevention adjustments #29092

Open
wants to merge 4 commits into
base: master
Choose a base branch
from
+27 −9
Open

Plane: Attitude stall prevention adjustments #29092

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
e3ac2d1
Plane: Fixed formula for load factor
abaghiyan Aug 4, 2023
089f577
AP_TECS: Corrected formula for _TASmin according to fix in formula fo…
abaghiyan Aug 4, 2023
d5ccb9c
TECS: Add getters for pratical stall airspeed
rubenp02 Jan 20, 2025
f1a99ec
Plane: Use stall speed for roll stall prevention
rubenp02 Jan 16, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
14 changes: 9 additions & 5 deletions ArduPlane/Attitude.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)) {
Expand All @@ -657,7 +659,9 @@ 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(TECS_controller.get_stall_airspeed_1g(), 1));

if (max_load_factor <= 1) {
// our airspeed is below the minimum airspeed. Limit roll to
// 25 degrees
Expand All @@ -668,13 +672,13 @@ 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(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);
}
}
}
2 changes: 1 addition & 1 deletion ArduPlane/Parameters.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
2 changes: 1 addition & 1 deletion ArduPlane/Plane.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
4 changes: 2 additions & 2 deletions libraries/AP_TECS/AP_TECS.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down
14 changes: 14 additions & 0 deletions libraries/AP_TECS/AP_TECS.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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();
}

Comment on lines +80 to +86
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'm not a fan of calling this the stall airspeed. AIRSPEED_MIN is explicitly meant to NOT be the stall airspeed, as per its parameter definition.

As a side-note, the resulting maximum loading factor is the maximum allowed loading factor, not the maximum loading factor supported by the airframe. That is what would correspond to the stall airspeed.

Copy link
Contributor Author

@rubenp02 rubenp02 Jan 21, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I understand, but if AIRSPEED_STALL is defined, this is going to be much closer to the actual stall speed. Since the idea behind AIRSPEED_STALL is to optionally allow for maneuvers that are closer to the stall speed, and the related math (the maximum loading factor and the scaling with bank angle) is canonically done based on Vstall, we're indeed considering AIRSPEED_MIN to be the stall airspeed in the code as it stands. And I was embracing that here because it's something that makes sense from a safety perspective and considering how poorly most AP aircraft are tuned. It makes a lot of sense to consider the minimum navigation speed (AIRSPEED_MIN) to be the stall speed in the TECS code unless the user explicitly defines an actual stall speed because that works out perfectly for both plug and play and more advanced users.

As a side-note, the resulting maximum loading factor is the maximum allowed loading factor, not the maximum loading factor supported by the airframe. That is what would correspond to the stall airspeed.

But the min. airspeed increase based on bank angle isn't something that makes any sense for an airspeed that isn't a stall speed.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@rubenp02 I think you're onto something.
But first, some clarifications.

Currently, this is what happens (to my understanding):

  1. Attitude.cpp::update_load_factor() will calculate a load factor using AIRSPEED_MIN.
  2. It will then communicate a load factor to TECS.
  3. In turn, TECS will increase _TASmin (its minimum airspeed limit) by:
  4. AIRSPEED_MIN*_load_factor
  5. (or if defined) max(AIRSPEED_MIN, AIRSPEED_STALL*_load_factor)`.

If I read your code correctly, you want to take the final TECS::_TASmin and feed it back into update_load_factor().
Won't this create a feedback loop, constantly increasing _TASmin?
I don't think that's right.

I think you have some more errors in your logic:

Since the idea behind AIRSPEED_STALL is to optionally allow for maneuvers that are closer to the stall speed

This is not correct. The idea is the allow maneuvers at AIRSPEED_MIN, as introduced here.

But the min. airspeed increase based on bank angle isn't something that makes any sense for an airspeed that isn't a stall speed.

Indeed. And as far as I understand, the PR linked above fixed that.

However, I think I see the merit in your idea.

Are you proposing switching the load factor calculation based on AIRSPEED_STALL, when that's available?
If yes, I think it's fine to repeat a similar if (is_positive(aparm.airspeed_stall)) structure in update_load_factor().
And I understand that will have the effect of not exaggerating the load factor.

Copy link
Contributor Author

@rubenp02 rubenp02 Jan 21, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Currently, this is what happens (to my understanding):

  1. Attitude.cpp::update_load_factor() will calculate a load factor using AIRSPEED_MIN.
  2. It will then communicate a load factor to TECS.
  3. In turn, TECS will increase _TASmin (its minimum airspeed limit) by:
  4. AIRSPEED_MIN*_load_factor
  5. (or if defined) max(AIRSPEED_MIN, AIRSPEED_STALL*_load_factor)`.

If I read your code correctly, you want to take the final TECS::_TASmin and feed it back into update_load_factor().
Won't this create a feedback loop, constantly increasing _TASmin?
I don't think that's right.

Take this with a grain of salt as I'm still learning the codebase, but I don't think that's true. The load factor is based on the demanded roll so it's entirely independent from the airspeed and thus no feedback loop is possible. To put it a different way, in TECS we're essentially doing:

true_airspeed_min_or_stall = true_airspeed_min_or_stall * sqrt(1 / cos(demanded_roll))

And in the attitude controller:

roll_limit = acos(1 / (current_airspeed / airspeed_min_or_stall)^2)

One side is adjusting the min. speed and the other the max. roll, so they don't interfere with each other.

Since the idea behind AIRSPEED_STALL is to optionally allow for maneuvers that are closer to the stall speed

This is not correct. The idea is the allow maneuvers at AIRSPEED_MIN, as introduced here.

As I understand it, it does do what I said even if that wasn't the intention, and it seems quite useful. If AIRSPEED_STALL is unset it does exactly what it did before that param. existed, and if it's set, it uses it in AIRSPEED_MIN's place. Since _STALL is lower than _MIN, _TASmin will end up being less conservative. This exact behaviour is one of the things this PR attempts to apply to the roll stall prevention, based on the same AIRSPEED_STALL parameter.

But the min. airspeed increase based on bank angle isn't something that makes any sense for an airspeed that isn't a stall speed.

Indeed. And as far as I understand, the PR linked above fixed that.

But the AIRSPEED_MIN still gets compensated for bank angle if AIRSPEED_STALL isn't set.

Ultimately where I want to go with this is to have a single source of truth regarding the stall speed as far as AP is concerned (i.e. as conservative or as precise as the user has configured it). This will be very useful to improve the stall prevention functionality, and to avoid future mistakes (this part of the code was riddled with uncanonical formulas and inconsistencies when it came to compensating for bank angle).

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Take this with a grain of salt as I'm still learning the codebase, but I don't think that's true. The load factor is based on the demanded roll so it's entirely independent from the airspeed and thus no feedback loop is possible.

Let's see:

Attitude.cpp indeed calculates aerodynamic_load_factor based on demanded_roll. And you also propose

float max_load_factor = sq(smoothed_airspeed / MAX(TECS_controller.get_stall_airspeed(), 1));
if (max_load_factor <= 1) {
// our airspeed is below the minimum airspeed. Limit roll to 25 degrees.
[...]

Consider the following reasoning:

  1. You fly level at $V_c$ that equal to AIRSPEED_MIN.
  2. A 45deg bang is requested and aerodynamic_load_factor=1.41.
  3. Currently, max_load_factor=1
  4. At this point you're not allowed to bank more than 25deg, because of the above code.
  5. You bank at 25deg.
  6. TECS raises _TASmin by sqrt(1.41).
  7. [next iteration]
  8. The demanded_roll is the same, so is aerodynamic_load_factor.
  9. max_load_factor is now even smaller, because the denominator has grown. Thus the roll will keep getting limited at 25deg. This is a steady-state point for this algorithm.
  10. Thus, the desired aerodynamic_load_factor does affect the calculation of max_load_factor. Albeit, in a more conservative way, which I think is the opposite of what you wanted to achieve?

Can you check my logic above?
In any case, one of your best arguments is going to be SITL simulations with a before/after comparison.

Copy link
Contributor Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That was what I intended (and I thought would be more accurate), but I was wrong because the denominator on the max. load factor formula should be the stall speed at 1G, i.e. without taking the increase from currently being in a bank into account, which was a pretty big mistake. Fixing that will also solve this issue you mention (it will be equivalent to what was done before, except for using the stall speed param. if available), and it will make this PR quite trivial.

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Okay, let's see it!

// 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;
Expand Down
Loading