NAV_L1: should wrap the Nu calculation?

[rmackay9]

PR #19975 suggests that we should wrap the results of the line shown below of AP_L1_Controller:

Nu = Nu1 + Nu2;

Should be wrapped like below or we can end up with "big turns":

Nu = wrap_PI(Nu1 + Nu2);

I suspect this is true but we need to reproduce the underlying issue (using master) and then show that this resolves the problem.

In case it helps we have a description of how the L1 controller works for Rover here on the dev wiki.

[xianglunkai]

   xtrackVel = _groundspeed_vector % AB; // Velocity cross track
   ltrackVel = _groundspeed_vector * AB; // Velocity along track
   float Nu2 = atan2f(xtrackVel,ltrackVel);

when _groundspeed_vector and AB are opposite and close to parallel,then Nu2 will jump between -PI and +PI caused by using atan2

[rmackay9]

@xianglunkai,

Txs for that. So I guess this can happen if the vehicle is travelling in the opposite direction from the segment. So I guess a problem could occur if Guided mode is used and "fly to here" is set to a location behind the vehicle.. but I've never actually seen a problem.

Do you have some detailed steps we can follow in the simulator to show the problem?

[xianglunkai]

L1 big turn pr

[rmackay9]

@xianglunkai, awesome, thanks! So I think you setup a mission with parallel lines and then forced the active waypoint forward. I'll try and reproduce in SITL. love it.

[rmackay9]

@xianglunkai,

I'm afraid I still can't reproduce the problem. One thing I don't understand about the picture is why is the boat turning so far from the next waypoint? Have you set the WP_RADIUS to be really large?

If possible could you include a log of the test?

By the way, you might be interested to test the Rover SCurves PR #19220 which removes the L1 controller completely.

[IamPete1]

I think we should certainly be wrapping this line:

ardupilot/libraries/AP_L1_Control/AP_L1_Control.cpp

Line 316 in a34f8e1

_nav_bearing = atan2f(AB.y, AB.x) + Nu1; // bearing (radians) from AC to L1 point

I'm not sure that warping Nu directly is a good idea, it will change the behavior of this limiting code:

ardupilot/libraries/AP_L1_Control/AP_L1_Control.cpp

Lines 322 to 323 in a34f8e1

	//Limit Nu to +-(pi/2)
	Nu = constrain_float(Nu, -1.5708f, +1.5708f);

[xianglunkai]

I do not think that wraping( Nu = wrap_PI(Nu1 + Nu2)) will change the behavior of the limiting( Nu = constrain_float(Nu, -1.5708f, +1.5708f).
In fact, this fixes the correct sign for lateral acceleration.
By the way , the log cannot be uploaded because it is too large (31M)

.

[rmackay9]

@xianglunkai, maybe you could put the log on some service (google drive, dropbox, etc)? There are a million services these days.

I guess you've added a patch which marks the waypoint as complete if the water depth has gotten too low? I'd be interested in seeing the patch if you're happy to share. I've certainly wanted this type of behaviour although I was thinking of a different approach where we would set the waypoints based on the shoreline (see mapping mode issue here)

[rmackay9]

Another small issue we can see with L1 is the "_last_Nu" variable used by _prevent_indecision() is never cleared meaning it could be left over from a previous segment executed last in the past.

[xianglunkai]

https://github.com/xianglunkai/ardupilot/tree/sf_usv