Rudder control for fishing vessel

Hi guys,

I have restarted a wee project that got put on the back burner a while ago.

I have a kontiki, that has a servo, GPS unit and compass.

Im pretty sure last time I was looking at my code I had got to the point where I had all my navigational aspects working.
I calculated a waypoint 100m offshore 90 degrees to the beach face using GPS and some spherical earth calcs.

I call my compass to calculate the current bearing the kontiki is travelling at.
Then I I call my GPS and compare the start position with the current position, calculate a heading to the original waypoint and compare is to my current heading.

The difference in the hearing to required bearing is feed to the servo to adjust the rudder accordingly.
Well that’s my thinking anyway.

I am a novice programmer in arduino, but believe i require a PID loop to control everything.

It would be amazing if someone could point me in the right direction, and how to structure all the code so it can update and check the heading to bearing every 20s or so