Update:
I've continued working on this over the past year and a half, and have made some solid progress.
Hardware:
The original version posted here used an arduino micro soldered onto a daughter board that had the sensors + servo board. That didn't last long, as I ran out of code space, and inputs. I've moved to a fully-custom board by taking the Arduino Mega reference design, and adding my sensors and servos to it. I also stripped out the power management it had and replaced it with my own. It now has two SMPSs to provide a +6V rail for the servos and a +3.3V rail for everything else. It also gave me access to dedicated hardware serial ports for comms and GPS
Software:
A lot of tuning happened here. It still has a ways to go:
It can finally make it down a multi-waypoint track, but as you can see, it does get lost quite a bit. The main remaining issue is crossing the wind (tacking, or staying out of irons to use sailing terms). The wind was blowing from the west this day, and you can see long stalls any time the boat got close to pointing that way.
Debugging is difficult because the boat does need to be in water, and outdoors (because of the GPS), which becomes very weather dependent. I also don't typically bring programming equipment out because I don't want to open the waterproof electronics case while near water. That's probably the single biggest reason it's taken this long to make serious progress.
That said - it's still alive!
I'm keeping everything on Github, so feel free to look / use: