differential: speed setpoint feasibility and slow down effect in mission mode

[chfriedrich98]

Solved Problem

This PR adds these 2 updates to the differential rover module:

1. The controller prioritizes yaw rate over forward speed. A high yaw rate setpoint can therefor make the forward speed setpoint infeasible due to actuator limitations. This leads the integrator of the speed controller to build up. This problem is fixed in this PR by adjusting the speed setpoint based on the amount of control effort that is “occupied” by the yaw rate setpoint.

2. Adds a tunable slow down effect in mission mode. As the rover approaches a waypoint it will slow down with the following logic: If the angle between the prev-curr-wp and curr-next-wp lines is bigger than RD_TRANS_DRV_TRN the rover will come to a full stop at the waypoint and then turn on the spot (this is not new). If this angle is lower then it will slow down s.t. it arrives at the waypoint with the following speed (this is new):
   $v{transition} = v{max} \cdot (1 - \theta_{normalized} \cdot k) $

   with

   • $v_{transition}:$ Transition speed
   • $v_{max}:$ Maximum speed 'RD_MAX_SPEED`
   • $\theta_{normalized}:$ Angle between the linesegment of prev-curr and curr-next waypoints normalized from $[0\degree, 180\degree]$ to $[1, 0]$
   • $k:$ Tuning parameter RD_MISS_VEL_GAIN This adds the new parameter RD_MISS_VEL_GAIN and also deprecates RD_MISS_SPD_DEF

Test coverage

• Tested in SITL
• Tested on hardware