AutoLineSmoothVel refactoring & bugfixes, 2nd Attempt

[jkflying]

Describe problem solved by this pull request
2nd attempt at #13460 , which had a bug which made the vehicle fly faster than desired.

This is mostly refactoring, but it also makes the following minor behavior changes:

1. Z velocity in missions is now controlled optimally, rather than with MPC_Z_TRAJ_P (which is now gone). This may result it slightly more aggressive Z velocities, but it will now actually follow the optimal Z jerk/accel/velocity profiles proscribed.
2. Z velocity and X/Y velocity are now synchronized for 3D waypoints. This means the vehicle will now fly straight lines between 3D waypoints of different altitudes, instead of having Z and X/Y decoupled.
3. 3D waypoints with altitude changes should no longer overshoot.

Describe your solution
The waypoint speed constraint logic was refactored to allow theoretically any number of setpoints to be chained together. This allowed the other logic fixes to be easily implemented, and will allow the obstacle avoidance interface to make use of this in the future as well, via changing the targets rather than the setpoints.

Test data / coverage
Tested extensively in SITL, and a number of unit tests added on the code as it was refactored.

@PX4/testflights could you please test fly this? It needs several different shaped mission flights.

Please add large altitude changes part-way through the flight. eg. on a straight line, add a 20m altitude change.

Also, please note that the altitude change might be more aggressive now, so you might have to reduce the MPC_ACC_DOWN_MAX / MPC_Z_VEL_MAX_DN if there is instability during descent.

[dannyfpv]

Tested on Pixhawk4 v5 f-450
Modes Tested
Mission mode: no issues
Default: MPC_ACC_DOWN_MAX / MPC_Z_VEL_MAX_DN
log:
https://review.px4.io/plot_app?log=ff8544c5-6eb3-4567-9e1a-f6bcb361bab9
https://review.px4.io/plot_app?log=47e3b1a9-8eb1-4209-b7f4-6b98f9e56a21
https://review.px4.io/plot_app?log=cfc54d6e-0089-4f29-bf36-f9a43d5f8b82

Reduced: MPC_ACC_DOWN_MAX = 10 / MPC_Z_VEL_MAX_DN = 2.0
log
https://review.px4.io/plot_app?log=d2b9691f-5f9b-47fa-92b4-b0e66547cd01
https://review.px4.io/plot_app?log=c9a0da26-20e7-4062-85fa-859c5c1fa94f
https://review.px4.io/plot_app?log=2cf57330-a6dd-47f5-8c94-f8af07316f86

Observations:
We didn't notice much of a difference by reducing the parameter, MPC_ACC_DOWN_MAX / MPC_Z_VEL_MAX_DN.

[jorge789]

Tested on PixRacer V4:

MPC_ACC_DOWN_MAX =3
MPC_Z_VEL_MAX_DN =1

A mission was made with change of altitude from 20m to 40m

• QGC armed form.
  It took off in mission plan mode
  . Note that the commands were sent from QGC (Armed and Takeoff)
  Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
  Good flight overall.

Log: https://review.px4.io/plot_app?log=05394dde-32c1-440d-bdcd-05d6a2aafe9d

A mission was made in a straight line with change of altitude.armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=5720d298-4005-4f95-88b1-710d657ba171

A mission with altitude change was carried out. Armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=205be5e3-145f-4122-9333-a162ffe88d10

Tested on PixRacer V4:

MPC_ACC_DOWN_MAX =2
MPC_Z_VEL_MAX_DN =.500

A mission was made with change of altitude from 20m to 40m

• QGC armed form.
  It took off in mission plan mode
  . Note that the commands were sent from QGC (Armed and Takeoff)
  Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
  Good flight overall.

Log: https://review.px4.io/plot_app?log=d1df9438-0bdc-444b-9b5d-e1f54a3d3499

A mission was made in a straight line with change of altitude.armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=7cfb0d0b-2464-4740-8f4e-40c63473fdbf

A mission with altitude change was carried out. Armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=c58ae023-a933-4958-bc95-2a33c32224e2

Observations:
We didn't notice much of a difference by reducing the parameter, MPC_ACC_DOWN_MAX / MPC_Z_VEL_MAX_DN.

[Junkim3DR]

Tested on NXP FMUK66 V3:

MPC_ACC_DOWN_MAX = 3
MPC_Z_VEL_MAX_DN = 1

A mission was made with change of altitude from 20m to 40m

• QGC armed form.
  It took off in mission plan mode
  . Note that the commands were sent from QGC (Armed and Takeoff)
  Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
  Good flight overall.

Log: https://review.px4.io/plot_app?log=30ad89e1-f35c-414f-8df1-4df0d91e7773

A mission was made in a straight line with change of altitude.armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=c48f2142-f534-42ab-a8f5-0d450792b607

A mission with altitude change was carried out. Armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=77a3b3b8-86e2-47b3-a46d-feb5aeb8b1b0

Tested on NXP FMUK66 V3:

MPC_ACC_DOWN_MAX = 2
MPC_Z_VEL_MAX_DN = .300

A mission was made with change of altitude from 20m to 40m

• QGC armed form.
  It took off in mission plan mode
  . Note that the commands were sent from QGC (Armed and Takeoff)
  Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
  Good flight overall.

Log: https://review.px4.io/plot_app?log=d3735225-b09f-4eb6-bd2e-b57d18bc4dfa

A mission was made in a straight line with change of altitude.armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=07e92257-2fd8-40f4-bab7-f0c513bdc85f

A mission with altitude change was carried out. Armed form of mi-QGC.
It took off in mission plan mode
. Note that the commands were sent from QGC (Armed and Takeoff)
Once the entire reference point was completed, the vehicle changed to RTL and landed as expected.
Good flight overall.

Log: https://review.px4.io/plot_app?log=603243dd-8d8b-409f-b230-28df55ab69b4

Observations:
We didn't notice much of a difference by reducing the parameter, MPC_ACC_DOWN_MAX / MPC_Z_VEL_MAX_DN.

[bresch]

Awesome work! This will help a lot for obstacle avoidance and we can soon ask Navigator to provide more waypoints and perform the actual switching in the FlightTask.

Just a few style comments, but it looks good for the rest. I also had a quick look at the logs of the test team and I didn't see anything weird.

[jkflying]

@mrivi any idea why the safe landing planner SITL is failing here? Maybe we can discuss in more detail the control flow there to make sure this isn't breaking anything?

[bresch]

@jkflying any news on that?

[jkflying]

any news on that?

I spoke with her offline, I need some time to discuss with you how this stuff interacts with something setting a setpoint (not waypoint) that isn't on the flight plan. Maybe we need to make it a 4-point optimization with the first being the setpoint? Anyway, it works well as long as the avoidance interface isn't involved, but needs a little rethink about how that fits in.

[jkflying]

Ok, after further discussion with @bresch it seems possible to change the avoidance interface to use the waypoint instead of the setpoint (so no changes here), we just need to check what is being sent to the avoidance so that it doesn't end up following its own waypoints thinking they are mission waypoints.

[jkflying]

@bresch can you take another look, short of reworking the entire obstacle avoidance interface I think this is the best I can do here. Longer term, I think reworking the obstacle avoidance interface is the right move though.

[jkflying]

Successful flight testing, at higher speeds too.
Here is one of the test flights:
https://logs.px4.io/plot_app?log=4f53ed8f-3edf-4f7e-beb3-4d6e046313e7

[MaEtUgR]

I made some space by removing the FlightTaskAutoLine and caused conflicts by merging #13701 , let me quickly rebase, I know what I changed.

[bresch]

I flew this PR this morning and I didn't find any issue

[MaEtUgR]

I resolved the conflicts, they were cleanups in files that I removed in the last commit. Also I removed the merge commit for master that was in between.

[bresch]

I made some more tests with @baumanta on a drone running obstacle avoidance and the result was really nice and smooth.
The old jerky auto mode and the duplicated AutoMappers are gone, I'm really happy to merge this PR, thanks again @jkflying .

[baumanta]

https://review.px4.io/plot_app?log=fedaa44a-7b74-450e-87f0-c450b5e36e04