This entire feature only has an impact if the last mode set
a huge acceleration and we have to take over as smooth as possible.
But it's stil lworth fixing.
The drag is based on max_acc/max_vel, which means that increasing the
maximum velocity leads to slower braking (at the same starting speed).
Especially a combination of small max_acc (slow responsiveness) with high
max_vel led to an exceedingly high braking distance.
This improves that while still being smooth.
The velocity setpoint of the position controller
does a jump when unlocking position with a non-zero position error.
This is solved by using the velocity setpoint feedback to smoothly
take over.
With a higher responsiveness, after centering the stick, the velocity and
acceleration setpoints could oscillate around 0 and never reach 0, due to
discretization.
This also prevented position lock engagement.
Daniel Agar