Ackermann speed from mission

[sbtjagu]

Solved Problem

The speed setted by a mission was never used.
Moreover after the previous refactoring the distance between the rover and waypoints was not updated correctly.
Thus making the desired speed locked at corner speed and breaking the delay command detection.

Solution

Mission speed usage :

• When updating waypoints from position_setpoint_triplet we get the desired speed if it is lower than _param_ra_miss_vel_def in airframe, and use it instead.

Distance update :

• Moving distance update from updateWaypointsAndAcceptanceRadius to computeGuidance

Test coverage

• SITL testing

Context

Before :

https://logs.px4.io/plot_app?log=ae0bb8f4-33a1-475c-8775-2324e9f51efe

After :

https://logs.px4.io/plot_app?log=0f6779c8-02d4-4029-9802-7004f81d3e9e

[chfriedrich98]

These changes look good to me, thanks for the implementation!
I just have one small change to adress.