If your goal is to hold a specific heading, the solution is not auto mode. You should try stabilize mode or depth hold modes, the autopilot runs it’s own pid controller to maintain the heading in those stabilized modes. Your program can give yaw input until the heading is where you want it, then just set the input to zero (the input is the desired rate of rotation), and the autopilot will take care of holding its current heading.