Smooth transitions to stabilized modes

Make transitions from manual to stabilized modes more smooth. E.g. if copter goes at some speed in SEATBELT and switched to EASY, setpoint will be adjusted to stop slowly and avoid returning back.

Tested in HIL. Need some flight testing and maybe some dependency on MPC_XXX_VEL_FF parameters.