Incresed control loop time. Small code refactor