Bug in monitorVelocity() was causing thread to sleep before unlocking it’s mutex, forcing the whole program to wait for several milliseconds. Fixed.