Rover: SCurve navigation

[rmackay9]

This PR adds support for SCurve navigation for Rovers and Boats which improves navigation including cornering. The most obvious change users will notice is how the vehicle cuts the corner rather than overshooting the waypoint. Either method is arguably correct but using SCurves also means that the route through the corners is planned while in master the controllers are essentially "surprised" by each corner in that they only ever have a single target and so when the vehicle reaches a waypoint they are simply handed a new waypoint and so they suddenly have a huge error they try to reduce. I.e. In master the corners are handled in a more reactive manner while with SCurves they are planned.

The high level changes are:

• AR_WPNav replaces L1 controller with SCurves which provide the desired position and velocity
• AR_PosControl controls how the vehicle gets from the actual position + velocity to the desired position+velocity

There are some addition drive-by changes to parameter defaults and Rover behaviour. These will eventually be separated out into separate PRs.

Remaining To-Do items:

• Figure out why Ackermann steering vehicles sometimes gets stuck at back-and-forth waypoints (i.e. 180deg turn) -- no longer getting stuck.
• Pivot turns should be improved by implementing with SCurves or "input shaping". -- we can resolve this in a future PR. the new behaviour is still an improvement on master.
• Resolve stopping at end of missions (seems to move around too much)

Completed To-Do items:

• Resolve autotest failures
• Detect and handle when user uploads mission while vehicle is in Auto mode (see equivalent code in Copter)
• add position and velocity PID info to GCS_PID_MASK to ease real-time tuning

Testing required:

• test various speeds and compare path

Alpha testing thread is here

[mikerob]

Hi @rmackay9 looks very interesting!

You refer to the "PosControl" starting to bring in velocity control - are you looking at introducing a longitudinal acceleration controller rather than / in addition to the speed controller, so there's some control over how a vehicle accelerates and decelerates?

I'm very curious to see how this plays out. One of the challenges with the current nav system is that it's hard to get the vehicle to track smoothly through a predictable path through a corner without running a lot of maths - working out your speed, desired lateral acceleration then calculating the turn radius you want and then setting WP radius to give you that without overshoot!!

A couple of test cases to look at will be how the new one handles corners of much less than and much greater than 90 degrees, and how closely you can place waypoints to control your path around a specific corner (i.e. following a road).

Is there a paper you're working from? I'd be interested to read the theory.

[rmackay9]

@mikerob, txs for the feedback. I'm hoping that this change greatly improves navigation through corners for Ackermann steering vehicles (aka separate steering and throttle vehicles).

The SCurve feature (originally designed for multicopters) calculates the vehicle's path to the next waypoint and the one after that including the corner. This path includes the vehicle's target position, velocity and acceleration at each moment. The new PosControl class's purpose in life is to calculate the best target turn rate and speed to get the vehicle to stay on the path.

By the way, the lower level forward-back speed controller has some parameters to control how the vehicle accelerates and decelerates... this PR doesn't change that part of the code instead focusing just on the higher level navigation.

[rmackay9]

Added parameter conversion because WP_PIVOT_ANGLE/RATE_DELAY have been moved to the AR_PivotTurn class. Below is a screen shot from SITL testing showing that the parameter conversion works. Left side shows the param values from master (purposely set to odd but easy to remember values) and the right shows the values from this PR's branch.

[rmackay9]

I've begun alpha testing this change here: https://discuss.ardupilot.org/t/rover-s-curves-alpha-testing/79570

[mikerob]

Setting ATC_ACCEL_MAX to 0 seemingly causes the code to crash when you try to enter AUTO mode.

[rmackay9]

@mikerob,

Thanks for the report. I've added protection and rebased on master.

[IamPete1]

I wonder if we should be relaxing the position controller in this case? Were basically ignoring its return.

[rmackay9]

yes, you must be right. The proper solution would be to add a Sailboat specific version of AR_WPNav .. I might look into how difficult that is before adding the reset. txs again.

[IamPete1]

Not sure if this is supposed to be here?

[rmackay9]

Indeed.. I hadn't noticed that there's a method directly above that can be used instead. Copter is using Vector3f::dot() so I basically just copy-pasted that but now I see Copter could also be changed to use operator*. I suspect thought that Leonard would prefer to stick with "::dot()" because it's more clear than reusing "operator*()".

.. I don't feel strongly about it but I think I might just fix the comment above Vector2f::dot() but keep using it although it's slightly wasteful to have two methods that do the same thing.. ?

[IamPete1]

I also think using the dot method is easier to follow. But there is value in keeping the code base consistent. Maybe we can change them all....

[IamPete1]

This is not always the case, we can be a 100% motoring mode. I think we can just update it in real time?

[rmackay9]

Great, thanks very much for this. I've changed this to be:

        // sailboats without motors may travel faster than WP_SPEED so allow waypoint navigation to
        // speedup to catch the vehicle instead of asking the vehicle to slow down
        rover.g2.wp_nav.enable_overspeed(motor_state != UseMotor::USE_MOTOR_ALWAYS);

EDIT: sorry, I updated this a couple of times and I honed in on the easiest to understand comment and code. :-).

[rmackay9]

.. and as you say it needs to be updated each iteration.. clearly I need to think about this some more. txs.