AP_L1_Control: Add integral term to loiter navigation

[rubenp02]

AP_L1_Control: Add integral term to loiter navigation

Description

The lack of steady-state crosstrack correction in the loiter navigation procedure means that loiter navigation isn't accurate, particularly in smaller loiters.

This PR adds an integral term to the circling PD (now PID) controller. It has been split for ease of review from #29165. Screenshots showing the difference these changes make can be found there.

Changes:

1. Added an integral term to the loiter navigation PID controller in AP_L1_Control
2. Added navigation mode enum and member to keep track of the current nav. mode

[IamPete1]

I find it a odd that here were using Nu1 in radians and in the loiter case were using crosstrack error in meters.

[rubenp02]

Yeah I agree but I figured changing that could overcomplicate the patch, as standardizing it to meters seems like the best solution but also introduces an unnecessary functional change to the waypoint navigation. What solution do you propose? I'll happily add it

[tridge]

@IamPete1 should we have a new parameter for the loiter integrator gain?

[IamPete1]

@IamPete1 should we have a new parameter for the loiter integrator gain?

I think a new gain might make sense using the one gain for two things as this currently does is a little odd. It would also give users the opportunity to set 0 gain to disable the new feature.

[rubenp02]

@IamPete1 should we have a new parameter for the loiter integrator gain?

I think a new gain might make sense using the one gain for two things as this currently does is a little odd. It would also give users the opportunity to set 0 gain to disable the new feature.

I don't think having this feature disabled, specially by default, is advisable. Without it the aircraft just doesn't accurately fly loiters (except very large ones) at the rate the navigation controller is run.

As for having 2 separate gains, IMO it would be better to adjust this to use the same error metric for both waypoint and loiter navigation. This would solve what you mention in your code review, and also make it very reasonable to reuse the same gain.

What do you think?

[IamPete1]

As for having 2 separate gains, IMO it would be better to adjust this to use the same error metric for both waypoint and loiter navigation. This would solve what you mention in your code review, and also make it very reasonable to reuse the same gain.

If you can re-work the error metric that would be great. We just have to make sure that the existing gain still behaves the same (although we could potentially do a param conversion to apply a correction factor if we absolutely had to)

[tridge]

@rubenp02 I did some testing with @peterbarker on a bixler at Spring Valley in high wind today, and we got very strange loiter paths. Here is an example:

I reproduced this issue in SITL by setting up high wind with the standard SITL plane. This is what I get with this PR:

this is trying to do a NAV_LOITER_UNLIMITED about WP 2, and you can see it goes nowhere near WP2.
this is with the same parameters and mission with master:

it is still struggling, but is clearly a lot better than with this PR.
I've put the logs from this SITL test here:
https://uav.tridgell.net/tmp/L1_Integrator/
I've also put the logs from Peters bixler there

I suspect the issue is the old one for navigation of body frame vs earth frame for the integrator. The issue here is wind, which is an earth frame issue, but this PR focuses on a body frame integrator (if I have understood it correctly)

[tridge]

the change fails to handle high wind, see comments

[NDevDrone]

@tridge @rubenp02 something like this? Earth frame (local N/E). The vector used for the integrator is the NE displacement
from the loiter center to the aircraft.

Let C be the loiter center position, p(t) the aircraft position (both in the same local N/E frame).
Define the radial displacement vector:
r(t) = p(t) - C (meters, North-East is positive)

Radius error (earth-frame geometric scalar):
e_r(t) = ||r(t)|| - R (meters)
where e_r > 0 means outside the circle (too far from desired radius to center), and e_r < 0 means inside the radius.

Unit radial direction:
r_hat(t) = r(t) / ||r(t)|| (dimensionless)

Radial correction acceleration (points inward if outside, and outward if inside):
a_corr(t) = - r_hat(t) * [Kp * e_r(t) + Ki * I(t) + Kv*de_r/dt]
with integral sum I(t) uses e_r(t). If you don't want to have a separate parameter for I gain then you can do

Radial correction acceleration (points inward if outside, and outward if inside):
a_corr(t) = - r_hat(t) * [Kp*(e_r(t) + I(t)) + Kv*de_r/dt]

Would be good to keep anti-windup as well as bounds on when its active to constrain its impact when far from circle.

[rubenp02]

What a coincidence, I had just returned to this a couple days ago and have something pretty much ready! I'll see how well it fares with these issues.

Critically, I looked deeper into this whole thing and realized the issue isn't a lack of an integral term in the loiter controller (in fact, the centripetal term largely eliminates the need for one). Instead, for very uncoordinated aircraft (in terms of adverse yaw), the current track-normal (lateral) acceleration -> roll controller has no chance of achieving the lateral acceleration needed. It always turns in the correct direction but with an error proportional to the current sideslip: Since roll output is calculated in open loop using a formula that really only models coordinated turns, any sideslip will throw the aircraft outside the intended trajectory (or inside it if we were to have inwards sideslip, which I've never seen). This current PR forcefully fixed that by progressively commanding more and more lateral acceleration, which eventually made the actually achieved lateral acceleration match the original command. So it somewhat works in a finnicky way if capture modes were to be properly handled (they aren't either), but it's entirely wrong from a control perspective.

So the one stop, direct solution is to add some sort of feedback to the lateral acceleration controller. Because the model is almost entirely accurate (and as far as I can see, fully accurate for what it's trying to do in aircraft that have turn coordination perfectly tuned), my solution relies heavily on it and just implements a self-tuning feedforward inverse controller (an IFF or PIFF controller in PIDFF terms, where P is a degenerate term provided by plant inversion).

It seems to work really well, and I was just getting ready to close this and post that. I'll almost certainly be able to get around to it today. What's unclear is whether it fixes the specific failure mode @tridge is showing, but I suspect it likely does (the default SITL plane is significantly uncoordinated in turns. Some of our drones have no yaw control, so they're at the whim of passive stability for turn coordination and were also really susceptible to this).

Edit: I had also finally resumed work on the G limiting stuff which will probably make it possible to merge my other loiter navigation fixes. I think some part of all of this is bound to fix the high-wind issues, we'll see.

[rubenp02]

@tridge @rubenp02 something like this? Earth frame (local N/E). The vector used for the integrator is the NE displacement from the loiter center to the aircraft.

Let C be the loiter center position, p(t) the aircraft position (both in the same local N/E frame). Define the radial displacement vector: r(t) = p(t) - C (meters, North-East is positive)

Radius error (earth-frame geometric scalar): e_r(t) = ||r(t)|| - R (meters) where e_r > 0 means outside the circle (too far from desired radius to center), and e_r < 0 means inside the radius.

Unit radial direction: r_hat(t) = r(t) / ||r(t)|| (dimensionless)

Radial correction acceleration (points inward if outside, and outward if inside): a_corr(t) = - r_hat(t) * [Kp * e_r(t) + Ki * I(t) + Kv*de_r/dt] with integral sum I(t) uses e_r(t). If you don't want to have a separate parameter for I gain then you can do

Radial correction acceleration (points inward if outside, and outward if inside): a_corr(t) = - r_hat(t) * [Kp*(e_r(t) + I(t)) + Kv*de_r/dt]

Would be good to keep anti-windup as well as bounds on when its active to constrain its impact when far from circle.

Guidance fixes alone are not enough to fix this: see my previous comment. But they may be needed on top of this if what I propose doesn't fix this latest high wind issue.