Still working on a control loop for the boat heading