Implemented slots for speed calculation (Servo12c). Did some test for loop timing:
FMU needs a while to raise the priority of the loop, s.t. it can be executed in 20ms.