removed thresholds for closed loop setpoints and added minimum thresholds for yaw rate and speed measurements instead to avoid moving due to measurement noise
Improve the slow down effect and add support for speed change in mission mode.
Seperate code related to turning setpoints into motor commands into its own folder and refactor code.