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: Fixed formulas for load factor and roll, also corrected _TASmin in AP_TECS accordingly #29101

Open
wants to merge 2 commits into
base: master
Choose a base branch
from
+10 −7
Open

Plane: Fixed formulas for load factor and roll, also corrected _TASmin in AP_TECS accordingly #29101

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
e4a9c15
Plane: Fixed formula for load factor
abaghiyan Aug 4, 2023
94301f5
AP_TECS: Corrected formula for _TASmin according to fix in formula fo…
abaghiyan Aug 4, 2023
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
11 changes: 7 additions & 4 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,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));

Comment on lines -660 to +663
Copy link
Contributor

Choose a reason for hiding this comment

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

This doesn't look NFC to me.

Previously aerodynamic_load_factor and max_load_factor were comparable quantities:

    } else if (max_load_factor < aerodynamic_load_factor) {

But now max_load_factor is applied a square root, whereas aerodynamic_load_factor is raised to ^2.

Copy link
Member Author

Choose a reason for hiding this comment

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

I think your right, here is some working out If we concentrate on just the squaring and ignore the rest

Existing code:

$$aerodynamicLoadFactor = \sqrt{x}$$ $$maxLoadFactor = y$$

That makes the comparison:

$$y < \sqrt{x}$$

Which we can re-write as

$$y - \sqrt{x} < 0$$

Then we square it:

$$(y - \sqrt{x})^2 < 0$$ $$x - 2 \sqrt{x} y + y^2 < 0$$

The new equality is:

$$aerodynamicLoadFactor = x$$ $$maxLoadFactor = y^2$$ $$y^2 < x$$ $$y^2 - x < 0$$

The difference is:

$$2x - 2 \sqrt{x} y$$

So as you say the equality is not the same. I guess the next question is which is the correct value. I will have to do some more reading I think.

Copy link
Contributor

@rubenp02 rubenp02 Jan 27, 2025 •
edited
Loading

Choose a reason for hiding this comment

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

These changes are definitely functional! The old code wasn't typically off by much because these values are often close to 1, and most users are probably running very conservative AIRSPEED_MIN values, but the formulas are currently wrong.

This PR changes them to the canonical formulas based on well-established aeronautical principles:

  • Load factor is equal to 1/cosϕ.
  • Stall speed scales with the square root of the load factor, not linearly.

For example, if the aircraft is flying at 20% over its stall/min speed, the old code would underestimate the roll limit by nearly 30%.

Copy link
Member Author

Choose a reason for hiding this comment

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

@Georacer @rubenp02, I decided to just plot both, this is the python:


from matplotlib import pyplot as plt
import math
import numpy

airspeed = numpy.linspace(1,40,num=1000).tolist()

AIRSPEED_MIN = 9

demanded_roll = 85

# Old method
def get_roll_lim_old(speed):
    max_load_factor = speed / AIRSPEED_MIN
    aerodynamic_load_factor = 1.0 / math.sqrt(math.cos(math.radians(demanded_roll)))

    if max_load_factor <= 1:
        return 25

    if max_load_factor < aerodynamic_load_factor:
        return max(25, math.degrees(math.acos((1/max_load_factor)**2)))

    return demanded_roll

# new method
def get_roll_lim(speed):
    max_load_factor = (speed / AIRSPEED_MIN)**2
    aerodynamic_load_factor = 1.0 / math.cos(math.radians(demanded_roll))

    if max_load_factor <= 1:
        return 25

    if max_load_factor < aerodynamic_load_factor:
        return max(25, math.degrees(math.acos(1/max_load_factor)))

    return demanded_roll


old = []
new = []
for speed in airspeed:
    old.append(get_roll_lim_old(speed))
    new.append(get_roll_lim(speed))


fig = plt.figure()
plt.title('Comparison of old and new load factor calcs')
plt.plot(airspeed, old, label = "old")
plt.plot(airspeed, new, label = "new")
plt.legend()
plt.xlabel('Airspeed (m/s)')
plt.ylabel('Roll (deg)')

plt.show()

It results in:
image

So no change in behaviour. Did I make a mistake in my python?

At least I suspect the code is more complex than it needs to be. I don't know why we don't just calculate the max bank angle and constrain it, the result is the same. EG:

def get_roll_lim_easy(speed):
    max_load_factor = (speed / AIRSPEED_MIN)**2

    if max_load_factor <= 1:
        return 25

    return min(max(25, math.degrees(math.acos(1/max_load_factor))), demanded_roll)

Then the aero load factor is not needed at all for this calculation.

Copy link
Contributor

Choose a reason for hiding this comment

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

That's my bad, sorry, I completely forgot that it wasn't only the roll limit formula that was wrong but also the max. load factor one that it depends on. Since we're incorrectly squaring the max. load factor and then taking the square root, both mistakes cancel out. However, this PR is still a functional change because of the TECS side of things:

import math

import matplotlib.pyplot as plt
import numpy as np


TAS_MIN_LEVEL = 15
BANK_ANGLES = np.linspace(0, 85, 100)


def calculate_tecs_limits():
    load_factors = 1 / np.cos(np.radians(BANK_ANGLES))

    tas_min_old = TAS_MIN_LEVEL * load_factors
    tas_min_new = TAS_MIN_LEVEL * np.sqrt(load_factors)

    return tas_min_old, tas_min_new


def plot_results(tas_min_old, tas_min_new):
    plt.figure(figsize=(10, 6))

    plt.plot(
        BANK_ANGLES,
        tas_min_old,
        label="Old Method (TAS_min = n × TAS_min_level)",
        color="red",
    )
    plt.plot(
        BANK_ANGLES,
        tas_min_new,
        label="New Method (TAS_min = √n × TAS_min_level)",
        color="green",
    )

    plt.axhline(
        y=TAS_MIN_LEVEL, color="blue", linestyle="--", label="Level Flight Stall Speed"
    )

    sixty_deg_idx = np.abs(BANK_ANGLES - 60).argmin()
    plt.plot(BANK_ANGLES[sixty_deg_idx], tas_min_old[sixty_deg_idx], "ro")
    plt.plot(BANK_ANGLES[sixty_deg_idx], tas_min_new[sixty_deg_idx], "go")
    plt.annotate(
        f"Old: {tas_min_old[sixty_deg_idx]:.1f} m/s\nNew: {tas_min_new[sixty_deg_idx]:.1f} m/s",
        (BANK_ANGLES[sixty_deg_idx], tas_min_old[sixty_deg_idx]),
        textcoords="offset points",
        xytext=(-60, 30),
        ha="center",
    )

    plt.title("TECS Minimum Airspeed Compensation Comparison")
    plt.xlabel("Bank Angle (degrees)")
    plt.ylabel("Calculated Minimum Airspeed (m/s)")
    plt.legend()
    plt.grid(True)
    plt.ylim(0, 50)
    plt.show()


tas_min_old, tas_min_new = calculate_tecs_limits()
plot_results(tas_min_old, tas_min_new)

tecs-comparison

As for the attitude side of things, I still think it's better to have the correct formulas even if the calculations end up being correct with the wrong ones. For better maintainability and also in case that any of these values ever gets added to the flight logs (which could be useful).

Finally, the branching based on the aerodynamic load factor is unnecessary, but personally I think that it makes the code ever so slightly more clear. It makes it obvious that most of the time the roll isn't limited beyond the standard parameter limits.

Copy link
Member Author

Choose a reason for hiding this comment

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

However, this PR is still a functional change because of the TECS side of things

@rubenp02 you have the new load factor calculation with the old method in TECS. As they are in this PR there is no change in behaviour because TECS is also updated. Your calculate_tecs_limits should be this:

def calculate_tecs_limits():
    load_factor_old = 1 / np.sqrt(np.cos(np.radians(BANK_ANGLES)))
    load_factor_new = 1 / np.cos(np.radians(BANK_ANGLES))

    tas_min_old = TAS_MIN_LEVEL * load_factor_old
    tas_min_new = TAS_MIN_LEVEL * np.sqrt(load_factor_new)

    return tas_min_old, tas_min_new

So, I still think there is no change in behaviour as a result of this PR as it currently is. @Georacer @rubenp02 do you agree?

Of course there is a discussion to be had if we want to change the behaviour, but that can be a future PR.

Copy link
Contributor

Choose a reason for hiding this comment

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

My bad again, you're right, this is NFC.

Copy link
Contributor

Choose a reason for hiding this comment

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

@IamPete1 @rubenp02 sorry for the slow replies, it's been a little busy.
I'll try to reply when I can, but mind you I haven't blocked this PR.

if (max_load_factor <= 1) {
// our airspeed is below the minimum airspeed. Limit roll to
// 25 degrees
Expand All @@ -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);
}
}
}
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
Loading