Fixed long cycle time. Rewriting sonar IMU. TBC