Trajectory mission slow final waypoint approach

[MaEtUgR]

Describe the bug
When using the Jerk-limited trajectory (default, MPC_AUTO_MODE 1) the final approach to a waypoint for example but not limited to the return altitude in RTL is significantly slower than with the previous implementation. Don't get me wrong, I would not prefer the old implementation but I think it's something we need to fix with medium priority.

To Reproduce
It's also visible in SITL:

1. Fly e.g. 10m away from home
2. Trigger RTL while you're clearly lower than the return altitude
3. Pay attention to the vehicle reaching the return altitude
4. It it's a very continuous motion but it almost looks like it's breaking too early and then takes quite long to fly into the acceptance radius and continue. The same can be observed in other missions.

Expected behavior
It's clear that a jerky corner takes less time and we don't have an optimized trajectory around corners yet but it would be nice if the vehicle could break down what intuitively looks more spot on to the waypoint. Maybe that's just tuning.

Log Files and Screenshots
Should I create a comparison for approaching times?

Drone:

• First seen in SITL and default parameters
• Now tested in real world on a small racing platform

Additional context
Original trajectory based mission pr: #10746

[bresch]

@MaEtUgR Yes, the default gains of the "virtual pilot" (MPC_XY_TRAJ_P and MPC_Z_TRAJ_P) are 0.3 and really conservative to ensure stability on a wide range of velocities, accelerations and jerks parameters.

To decrease the time is takes to reach a waypoint, simply increase those gains.

[MaEtUgR]

Then the solution might be to simply adjust the default. But if SITL on master already shows that behavior and it wasn't there before I still consider it an issue.

[MaEtUgR]

#11841 Could be a viable solution.