Plane: Fix loiter navigation accuracy

[rubenp02]

Plane: Fix loiter navigation accuracy

Description

This PR addresses two major issues with loiter navigation in Plane:

1. Incorrect altitude compensation for the loiter radius
2. Lack of steady-state crosstrack error correction in loiter navigation

Altitude Compensation

The current implementation in master overcompensates for the effects of altitude on turn radius. It applies a correction factor that is only necessary for the tightest possible loiter to loiters of all sizes. This behavior was improved in #5909, but in a way that is specific to loiters and requires parameter setup to take advantage of.

As a result, loiters weren't performed at the correct radius except when flying close to sea level. Moreover, since TECS is based on TAS, no explicit altitude compensation is actually needed. Loiters naturally adjust in size depending on what the aircraft can handle at the current altitude (and always match the requested radius when possible).

This can be seen in the following graph:

The explicit altitude correction currently used in master and the implicit correction the TECS naturally provides are nearly identical. They both perform loiters at the requested radius until the altitude increase makes it impossible, and then adjust the radius to be as close as possible to the requested one while avoiding a stall. However, the implicit correction doesn't require any parameter setup, and the difference between the flown radius and the requested one is reflected in the flight logs navigation crosstrack error field, which can be useful.

Therefore, the explicit altitude correction should be removed. But since an approximation of the loiter radius the aircraft will achieve on certain conditions is needed for navigation, the loiter radius compensation has been reworked to provide this information, but without having a direct impact on navigation.

Steady-State Crosstrack Correction

Even after addressing the altitude compensation issues, the lack of steady-state crosstrack correction in the loiter navigation procedure means that loiter navigation still isn't accurate, particularly in smaller loiters. This PR adds an integral term to the circling PD (now PID) controller.

Changes

1. Removed explicit loiter radius altitude compensation.
2. Removed the NAVL1_LIM_BANK parameter. Now, the ROLL_LIMIT_DEG is assumed to be safely maintainable at sea level.
3. Reworked the loiter radius compensation to work for any given airspeed and altitude. This allows for more accurate compensation in the AUTOLAND mode, and also to determine if any given loiter is viable, which could be useful to implement a pre-arm warning about a given loiter mission item that is going to be flown with a larger radius, for example.
4. Added an integral term to the loiter navigation PID controller in AP_L1_Control
5. Added navigation mode enum and member to keep track of the current nav. mode

---

Screenshot comparison (all default params.)

Original navigation

Altitude issues fixed

Altitude issues fixed (smaller loiters)

Integral term added (smaller loiters)

[IamPete1]

I'm a fan of reworking the loiter radius calculation, the automatic scaling we do by default is confusing and makes mission planning hard. However, we can't just remove it. As your example showed the vehicle will still fly larger radiuses. Plane needs to know what the flown radius will be for navigation. The new autoland mode is a good example of this, the final waypoint is placed such that the tangent of the loiter lines up with the runway. If you try autoland mode at high altitude with this branch I suspect you will find a much worse result.

[rubenp02]

Got it. But there aren't that many instances where the effective loiter radius has to be known (they are just those that I have patched out in 6b5fa81 and a couple others). So, doesn't make more sense to remove the altitude compensation from the L1 code entirely, and to simply add a method to get an approximation of the effective radius at a given altitude to the Plane's navigation module, for the few times when it's needed?

[IamPete1]

As a result, loiters weren't performed at the correct radius except when flying close to sea level. Moreover, since TECS is based on TAS, no explicit altitude compensation is actually needed. Loiters naturally adjust in size depending on what the aircraft can handle at the current altitude (and always match the requested radius when possible).

I don't understand the mechanism here, what has TECS got to do with it? TECS is speed/height, should not affect roll/radius.

If you break out points 4 (formatting) and 5 (typo) into a new PR we could probably merge them relatively quickly and simplify this PR a little.

[rubenp02]

I don't understand the mechanism here, what has TECS got to do with it? TECS is speed/height, should not affect roll/radius.

Max roll is altitude independent, it only depends on speed. TECS automatically adjusts the speed for the altitude (it demands a higher TAS in proportion with the altitude), which keeps the flight envelope the same. So the roll limits stay the same, but since we're flying faster, the turn radius is going to be larger (for loiters and in general, as it should).

If we reach a point where we no longer have enough power to keep increasing the TAS, then the roll stall prevention will eventually kick in, further increasing the turn radius. And if we're close to stalling/have stalled, the underspeed protection will kick in and prioritise speed over altitude, so we basically can never stall due to high altitude if everything is correctly implemented and tuned. This means that no explicit code to scale the turn radius is needed, and IMO having it is a flawed approach.

If you break out points 4 (formatting) and 5 (typo) into a new PR we could probably merge them relatively quickly and simplify this PR a little.

Sounds good. But in this PR, I also have to change some parameter files and testing scripts to remove the NAVL1_LIM_BANK param., and also fix the navigation code with the method that approximates the effective turn radius.

[rubenp02]

Created a new PR with the formatting changes only at #29177. I will remove those commits from here when I get around to making the other changes that are needed.

[rubenp02]

This is still a WIP, but does it look good in regards to what you said about navigation, @IamPete1? And in general

[IamPete1]

Max roll is altitude independent, it only depends on speed. TECS automatically adjusts the speed for the altitude (it demands a higher TAS in proportion with the altitude), which keeps the flight envelope the same. So the roll limits stay the same, but since we're flying faster, the turn radius is going to be larger (for loiters and in general, as it should).

Makes sense thanks.

This is still a WIP, but does it look good in regards to what you said about navigation, @IamPete1? And in general

I don't think you need to try and fix the using the current altitude for lookup rather than the target altitude thing in this PR. The messing about with baro is upsetting CI.

I wonder if just using the roll/speed limit radius is the correct thing to do. It means that as soon as you have none zero wind you can't fly the circle anymore. Maybe we could give ourselves some margin so that the radius is a little larger but we will be able to track it better. I not sure.

There is quite a bit going on in this PR still, if you can break it up anymore that would make it easier to review and faster to get in. Could the addition of the I term be broken out into a new PR?

[rubenp02]

I wonder if just using the roll/speed limit radius is the correct thing to do. It means that as soon as you have none zero wind you can't fly the circle anymore. Maybe we could give ourselves some margin so that the radius is a little larger but we will be able to track it better. I not sure.

Completely agree. But for it to be rigurous, how much margin should be add? And where do you think the margin should be added? IMO for separation of concerns the radius correction logic should only account for the speed and altitude. The old version also didn't account for the wind, and couldn't be used with arbitrary speeds and altitudes. Aside from how the autoland code can now get the corrected radius at the proper altitude, this rework is functionally very close to it (it's just using the proper formula to calculate the same thing).

The messing about with baro is upsetting CI.

Haven't looked into it pretty much at all, but I think the issue is that one of the preprocessor branches doesn't build at all. That part is fully WIP at the moment.

I don't think you need to try and fix the using the current altitude for lookup rather than the target altitude thing in this PR.

I figured I'd do it because the loiter comp. rework makes it trivial. Or do you mean that I should also take that rework itself to another PR?

There is quite a bit going on in this PR still, if you can break it up anymore that would make it easier to review and faster to get in. Could the addition of the I term be broken out into a new PR?

If that's going to be better, sure! It can be broken up quite a bit, but there are some dependencies and I feel like conceptually it all belongs together. I was going to write some commit bodies and flesh out the PR description a bit more. But if that's not enough, I'll gladly break it up into several PRs.