L1: remove check for tangent_vel direction for nicer loiter entry behavior

[sfuhrer]

Based on discussion in slack.

Describe problem solved by this pull request
The current L1 logic often makes the vehicle first fly through center of loiter waypoint, instead of tracking the loiter circle as soon as it is close to it (or after already being inside the circle).

Describe your solution
Remove check for the tangential velocity to "prevent PD output from turning the wrong way". AFAIK this causes the vehicle to fly straight to the loiter center if the tangential velocity is only slightly in the wrong direction (e.g. because it's hitting the loiter circle at a 91° angle).

Describe possible alternatives
Instead of removing check completely, allow small tangential velocities in the wrong direction, e.g. tangent_vel < 1.0f.

Test data / coverage
Basic SITL testing.

Additional context
Before:

With this PR:

@priseborough can you shed some light why it is there? What are the conditions where it could turn the wrong way?

[dagar]

FYI @Antiheavy

[mrpollo]

@bresch ping

[Antiheavy]

I'm also unsure about the reasoning behind the original logic, although I'm curious to find out. I do know that PX4 fixed wing often has poor loiter entry performance and I suspect the original "only turn one way" logic is the main culprit. For example, if a strong wind is coming from the wrong direction it is possible for a vehicle to get stuck in an infinite loop (pun applies) as it tries to turn only one way to enter the circle, but the wind accelerates the turn so much that the aircraft gets blown off course and has to loop around and try again, and again, and again. I suspect removing the only turn one way logic might help.

[jkflying]

This makes a lot of sense to me, trying to be too strict means the possible behavior gets boxed into a very narrow band of possibilities, which ends up not being optimal.

Would it also make sense to increase the threshold where the loiter starts to ~2x loiter radius, so it enters the loiter with a nice arc of the same radius as the loiter?

[Antiheavy]

Would it also make sense to increase the threshold where the loiter starts to ~2x loiter radius, so it enters the loiter with a nice arc of the same radius as the loiter?

This may be nice for smaller loiters, but for large loiters (e.g. 1 km radius, I've seen it done) it might be less desired to come off the flight line to start heading to tangent so far away from the circle.

[jkflying]

I guess that additional distance would need to be calculated based on minimum turn radius then (speed + max bank angle), not the loiter radius. But it would clean up the loiter entrance a lot.

[sfuhrer]

Also valid point about optimizing the loiter entry point @jkflying , but let's first focus on removing these unnecessary flights through the center point - The entry looks also particularly bad in the screenshot above because the loiter radii are really small, it gets better when that's increased. Plus I've actually made some unfavorable changes to that logic, with default master it looks better. That's unrelated to the direction/fly-through-center issue though.

[priseborough]

The original concern was that of the vehicle was entering the loiter in reverse direction, it would turn the wrong way initially. It is interesting that this check is still used here https://github.com/ArduPilot/ardupilot/blob/02daa4c3ecbe6f94e92d6dfb14b6deda24f4c8f9/libraries/AP_L1_Control/AP_L1_Control.cpp#L415

Testing should include a reversal of the loiter direction, ie transitioning from a RH loiter to a LH loiter WP at the same location, or by changing the parameter that sets the direction.

I would also evaluate the effectiveness of only running the check when _circle_mode = true rather than removing it.

[priseborough]

An alternative to removing this code block that should be evaluated is replacing this line with

if (tangent_vel < 0.0f && _circle_mode) {

So that the protection against getting trapped in a reverse direction loiter is preserved

[sfuhrer]

Changing it to if (tangent_vel < 0.0f && _circle_mode) { seems to do the trick:

With it:

Without:

I've also tested with reversing the loiter direction. Entry looks awful (guess that's no big surprise), but it consistently ends up doing it the circle in the correct direction.

[sfuhrer]

Something else I've realized though and is related: It always switches from circle_mode false/true a couple of times when entering the loiter. Don't think that's the idea. Should we add some kind of hysteresis, i.e. keep the condition to enter circle_mode the same, but only exit it again if the difference between lateral_accel_sp_center and lateral_accel_sp_circle is above some threshold?

lateral_accel_sp_center: -0.056682, lateral_accel_sp_circle:0.210643, xtrack_err_circle: 48.342255 circle mode false
**********************
lateral_accel_sp_center: -0.083798, lateral_accel_sp_circle:0.095527, xtrack_err_circle: 47.914680 circle mode false
**********************
lateral_accel_sp_center: -0.060450, lateral_accel_sp_circle:0.033607, xtrack_err_circle: 47.685593 circle mode false
**********************
lateral_accel_sp_center: -0.082510, lateral_accel_sp_circle:-0.097917, xtrack_err_circle: 47.200775 circle mode true
**********************
lateral_accel_sp_center: -0.064544, lateral_accel_sp_circle:0.000070, xtrack_err_circle: 47.028946 circle mode false
**********************
lateral_accel_sp_center: -0.086612, lateral_accel_sp_circle:-0.275091, xtrack_err_circle: 46.544182 circle mode true
**********************
lateral_accel_sp_center: -0.062366, lateral_accel_sp_circle:0.000065, xtrack_err_circle: 46.315048 circle mode false
**********************