AP_L1_Control: Add wind compensation for 90 degree turn distance

[menschel]

This is a Request for Comments for a Work in Progress Enhancement. Please add tags WIP and RFC respectively.

Abstract

Shift waypoint turn-in to compensate for wind.

Observation:

When a plane enters a 90 degree turn to the next waypoint while flying with tailwind, the plane overshoots and has to correct after the turn.
The overshoot is caused by the crosswind the plane experiences relative to the new course.

Goal:

Forward-correct the crosstrack error after the turn by an earlier turn-in.

Assumptions and classifications:

1. WP_RADIUS is bigger or equal to plane minimal radius (Physical constraint of the plane's turn ability)
2. Wind does not change direction during the turn (Physical constraint to establish forward error correction ability)
3. If the plane flys with tailwind at start of turn, it will overshoot the leg to the next waypoint (Conditional evaluation if correction is necessary)
4. The amount of overshoot is the "crosswind component of the new course" multiplied by the time needed for the turn (Error component).
5. The time needed for the turn is the distance traveled on WP_RADIUS divided by the ground speed (Error component).

Simplifications:

1. For a 90 degree turn, the tailwind at the start of turn is equivalent to the crosswind at the end of the turn.

[Ryanf55]

@magicrub

[IamPete1]

Seems like a reasonable change, may be better to do it at the plane level tho, it has a better idea where its going next.

[menschel]

OK, where would I take the current turn rate from and where alternatively would I apply this change on plane level instead of L1_Control?

[menschel]

Does it go into command logic instead of L1_Control?
https://github.com/ArduPilot/ardupilot/blob/master/ArduPlane/commands_logic.cpp#L658

[menschel]

I believe this PR is now mature enough to be reviewed. I'll try to add some test with SITL.

[Georacer]

L1 distance is already scaled by the groundspeed, which will increase as you have more tailwind.
Isn't your approach double-dipping on the scaling of L1 distance?

I admit however that this MIN( is forcing a max distance of WP_RADIUS, which can be too small, and masking the effect of the L1 scaling.

What I would like to see is some testing that shows:

1. The current behaviour in head and tailwind.
2. Your proposed PR in head and tailwind.
3. What would happen if you turned this MIN( into a MAX(, instead of using your new code.

[menschel]

Interesting Idea.
I can't say that I fully understand the L1 algo but with my limited knowledge I assume that L1 point will continue to update on the course and not stop at the waypoint.
The MIN(wp_radius, _L1_dist) seems to be an invariant for that.

This is an attempt to setup L1 Navigation correctly with 20km/h wind from the right and 35km/h cruise speed. Rectangle 300x100m. Regular Arduplane 4.5
It is clear that turning into the wind is beneficiary to perfectly line up with the next leg. While turning with the wind creates crosstrack error. Reviewing this picture also unveils, that the 90degree approach is not perfect, so I'll try to setup a SITL test first.