Trajectory generator-based FlightTasks - Handle position and velocity resets from EKF

[bresch]

So far, only the XY position reset was properly handled to avoid position jumps during an EKF reset (GPS regained for example). This PR also takes care about the Z axis and all the velocities resets.

Needs #11636 to show the real improvement.

[bresch]

@PX4/testflights please test this PR with a multicopter and the following parameter:
MPC_POS_MODE = 3 (Smooth Vel)
We need to test that both in manual position control and in mission.
Thanks!

[jorge789]

Tested on Pixhawk v2 cube; The MPC_POS_MODE parameter was changed to Smooth position control (velocity). It took off in position, but there was a delay between the RC commands and the vehicle response; note that this issue was only present during Position Mode.

In altitude, Stabilized, Mission and RTL modes vehicle flew as expected.

https://review.px4.io/plot_app?log=83f6e61d-3fa6-4b4f-9c52-1b12604e1af8

[Junkim3DR]

Tested on Pixhawk 4 v5; no issues were noted, good flight in general.

Parameter tested:

• MPC_POS_MODE to Smooth Position Control (Velocity).

https://review.px4.io/plot_app?log=4bf45cbe-6658-4fbd-913e-a4a0cdac519c#Nav-Local-Position-Y

[Tony3dr]

Tested on Pixhawk 4 v5 & Pixhawk v2 cube
slight delay in commands and the vehicle response in Position Mode for the V2. Other than that the vehicle flew as expected, good flights overall.

[bresch]

Thanks @jorge789 , @Junkim3DR and @Tony3dr .

I checked the log files:
On v2:
MPC_ACC_HOR_MAX = 2.5
MPC_JERK_MAX = 2

On v5:
MPC_ACC_HOR_MAX = 10
MPC_JERK_MAX = 20

So it makes sense that the response is slower on the V2.

@dagar Can we merge that PR?

[dagar]

Thanks everyone, let's get this in.