Plane: Fix loiter navigation accuracy #29165
Conversation
|
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. |
|
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? |
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. |
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.
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. |
|
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
changed the title
rubenp02
force-pushed
the
bugfix/fix-loiter-navigation
branch
from
c2ac218 to
79a9542
Compare
|
This is still a WIP, but does it look good in regards to what you said about navigation, @IamPete1? And in general |
Makes sense thanks.
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? |
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).
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 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?
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. |
|
I have checked just in case and the new altitude compensation code is indeed a perfect match of the old one when given the same parameters:
|
rubenp02
force-pushed
the
bugfix/fix-loiter-navigation
branch
2 times, most recently
from
019ba2b to
bd2ff71
Compare
rubenp02
force-pushed
the
bugfix/fix-loiter-navigation
branch
2 times, most recently
from
e8f07a9 to
5b28972
Compare
|
What do you think about this now @IamPete1? |
| const float corrected_loiter_radius = plane.nav_controller->loiter_radius(loiter_radius); | ||
| // corrected_loiter_radius is the radius the vehicle will actually fly, this gets larger as altitude increases | ||
| const float corrected_loiter_radius = | ||
| plane.nav_controller->calc_corrected_loiter_radius(loiter_radius, NAN, |
There was a problem hiding this comment.
Me neither! But overloading isn't a good solution here because the idea is to be able to pass the altitude, the airspeed, both or neither. That would be 4 overloads.
There was a problem hiding this comment.
But your not actually using all those combinations. I think its fine to even not pass altitude to start with, that is a existing issue that could be fixed in a separate PR.
There was a problem hiding this comment.
They aren't being used at this time, and neither is the is_loiter_achievable method, but since this is being reworked anyway I tried to make it as rigorous and flexible as possible. I think that's a good thing: when writing this code I got the impression that the current way this is handled is a bit sloppy, and more parts of the code would benefit from altitude or speed-specific compensation.
If the NaNs are an antipattern in ArduPilot (maybe -1.0f would be preferable?) I can get this down to 2 overloads, one using the current alt. and speed and the other using the specified ones. But any less than this and I don't see too much of a point in reworking the loiter compensation calcs.
Maybe this can be discussed in the dev call? Could I reach out to you by Discord to get some of my other PRs tagged for discussion as well, like this page says?
| @@ -145,36 +136,41 @@ float AP_L1_Control::turn_distance(float wp_radius, float turn_angle) const | |||
| return distance_90 * turn_angle / 90.0f; | |||
| } | |||
|
|
|||
| float AP_L1_Control::loiter_radius(const float radius) const | |||
| float AP_L1_Control::_calc_min_turn_radius(float indicated_airspeed, | |||
There was a problem hiding this comment.
Rather than the NaN, have two finction with a difrenct fingerprint. One takes two floats is the main funciton. Then have one with the same name that takes one float then fetches the alt and passes it into the other function.
There was a problem hiding this comment.
See my reply to the prev. comment
| // Reset integral error component if gain parameter has changed. This allows | ||
| // for much easier tuning by having it re-converge each time it changes. |
There was a problem hiding this comment.
That introduces steps in the output, better to keep it smooth.
There was a problem hiding this comment.
Although I see it is existing code. Seems a little unnecessary....
There was a problem hiding this comment.
I see the point of the code, it provides easier "real time" tuning and doesn't have an impact in normal flight.
rubenp02
force-pushed
the
bugfix/fix-loiter-navigation
branch
2 times, most recently
from
343b8a7 to
82724da
Compare
Extended the implementation of get_EAS2TAS_for_alt_amsl to use the simple atmospheric model when the 1976 model isn't available. This makes it possible to obtain the EAS2TAS factor for an arbitrary altitude outside SITL and regardless of the build type.
The TECS naturally provides turn radius compensation with altitude (not just in loiters but in general), so there's no need to explicitly correct for it in the loiter navigation code in particular.
Reworked the turn radius compensation calculations using the canonical turn radius formula. The new implementation also allows for the optional specification of altitude and airspeed at which the turn is to be performed. Key changes: - Added the baro and aircraft parameter objects as dependencies of the L1 navigation controller. - Introduced the calc_corrected_loiter_radius public method, that replaces the old loiter radius compensation method and optionally accepts altitude AMSL and indicated airspeed for a more flexible compensation. - Added is_loiter_achievable, which checks whether a given loiter radius is achievable. This can be useful to do sanity checks on loiter mission items, for instance. - Removed NAVL1_LIM_BANK parameter and instead relied on the assumption that ROLL_LIMIT_DEG is a bank angle that can be sustained at sea level. In practice, in a well tuned aircraft, this is always the case. This avoids the need for explicit configuration of the old approach.
Added navigation mode enum and member to keep track of the current mode. This makes it possible to run logic on mode changes.
Extracted the logic to calculate the integral term for waypoint navigation into a private method, and applied it to the loiter navigation.
rubenp02
force-pushed
the
bugfix/fix-loiter-navigation
branch
from
82724da to
29ce287
Compare
|
I think it would be better to do a simplify version of this first. No need current vs target altitude correction on the radius in this PR. Removing that makes the PR much simpler. Then we can circle back to that once the core functionality is in. |
Plane: Fix loiter navigation accuracy
Description
This PR addresses two major issues with loiter navigation in Plane:
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
Screenshot comparison (all default params.)
Original navigation
Altitude issues fixed
Altitude issues fixed (smaller loiters)
Integral term added (smaller loiters)