Stop Plane flying away in avoidance recovery (instead of loitering)

[peterbarker]

No description provided.

[peterbarker]

Flaw: if the vehicle is ascending or descending we will lock the new altitude into the guided waypoint with this patch.

[peterbarker]

Closes #15028

[peterbarker]

... this also leaves the default behaviour as "fly away 10km".

[peterbarker]

I have only tested this in SITL.

It switched modes to loiter when the recovery action was set appropriately.

It also stopped NTUN.Dist heading to very large values.

[samuelctabor]

This is definitely better. There is another problem however with a stationary threat - when resuming into loiter, the aircraft turns and goes back onto a collision course and this triggers the avoidance again. In this example it eventually led to flying around the stationary threat at a distance of approximately AVD_F_DIST_XY - using the following parameters.

AVD_ENABLE 1.000000
AVD_F_ACTION 3.000000
AVD_F_ALT_MIN 0.000000
AVD_F_DIST_XY 300.000000
AVD_F_DIST_Z 500.000000
AVD_F_RCVRY 3.000000
AVD_F_TIME 30.000000

[magicrub]

@peterbarker I remember you and I testing this a lot back when it first went in. Perhaps we should look back at the initial implementations to see how this was handled

[peterbarker]

On Mon, 17 Aug 2020, Samuel Tabor wrote: This is definitely better. There is another problem however with a stationary threat - when resuming into loiter, the aircraft turns and goes back onto a collision course and this triggers the avoidance again. In this example it eventually led to flying around the stationary threat at a distance of approximately AVD_F_DIST_XY - using the following parameters.

Yeah, I was afraid that might happen :-/ The other solution was to extrapolate that guided waypoint a little less. I like that because it makes me really uneasy having any state like that just lying around in the autopilot. Any suggestions on how we might project the guided waypoint a *sensible* amount? Has to account for really high winds (tail and head), really low airspeeds and really fast airspeeds. tridge agreed we should change the default recovery behaviour, BTW - i.e. "stay in avoidance" should actually loiter...

[magicrub]

This fixes the fly-away type behavior but what we need is some hysteresis (short timer?) so we don't toggle back and forth in/out of the avoid as we leave the radius and then re-enter. For the sake of things.. I suggest we merge this as-is because it fixes a critical fly-away bug and make a new PR to change the behavior of the algorithm re-entry issue

[magicrub]

Any suggestions on how we might project the guided waypoint a
sensible amount?

How about grab airspeed and project it 10 seconds away

[magicrub]

Per the DevCall meeting decision, we should change the logic from fixed 10k distance offset to a point that is 0.5*loiter_radius away from the obstacle keep-out boundary.

[samuelctabor]

I think to fix this would need -

• the target loiter point placed at 1x loiter_radius from obstacle keep-out plus some buffer.
• awareness in the avoidance code that when in loiter mode, the aircraft velocity vector doesn't need to be projected out by AVD_F_TIME when determining closest approach. (Ideally the closest approach would be determined from the loiter point and loiter radius subtracted).

[magicrub]

@samuelctabor yeah, 1x the loiter radius. In my head when I wrote that I was thinking diameter for some weird reason. A buffer is a good idea but it should be minimal. How about 1 second of flight per ARSPD_TRIM_CM.

I either don't understand or totally disagree with the second part. The param is there as a user-defined keep-out zone. Why would we not use it? It's always being computed anyway

[magicrub]

I talked to @samuelctabor and it was he who was not understanding :) I'll let him clarify.

[samuelctabor]

We're on the same page now I think, I was talking about placing the loiter point a distance from the obstacle itself, you were talking about distance a from the position where AVOID_ADSB is triggered.

[samuelctabor]

The behaviour seems similar to before when avoiding a stationary threat - rapid switching between LOITER and ADSB forming a circle around the obstacle. The radius it is teetering on is about 1300m so somewhat larger than AVD_F_DIST_XY. This time the parameters were:

AVD_F_ACTION 3.000000
AVD_F_ALT_MIN 0.000000
AVD_F_DIST_XY 1000.000000
AVD_F_DIST_Z 500.000000
AVD_F_RCVRY 3.000000
AVD_F_TIME 30.000000

I haven't looked at the code but I think what happens is this. It places the target point correctly during AVOID_ADSB, but then as soon as it turns to head to this point the trajectory no longer intersects the obstacle so it recovers to LOITER. Then, it starts turning, making the trajectory intersect the obstacle again and triggers AVOID_ADSB.

If this is correct either it needs to;

• stay in AVOID_ADSB until is outside of AVD_F_DIST_XY + AVD_F_TIME * TRIM_ARSPD_CM before recovering to LOITER
• be aware that when in LOITER mode it will not be going in a straight line for AVD_F_TIME - because it's circling - so there is no need to project the trajectory out by AVD_F_TIME.

[peterbarker]

I really think we need to put this in unless somebody has time to really work on the at-DevCall-proposed solution. I've just added a test which came up as part of a patch which also touches the existing avoidance code.

This is what happens on master with the test which is now attached to this PR:

I have no reason to suspect that Plane won't keep flying for a very, very long time.

... and this is post-this-PR: