Heading control with PID