My code is here.
It obviously won't work for you, because it's meant for a telescope, and as you said, they don't move like boats.
My point is, instead of the code you're using (which has so many trigonometric functions) you could try Pololu's driver.
It has a "heading" method which according to the documentation:
float LSM303::heading(vector from)
"Returns the angular difference in the horizontal plane between the 'from' vector and north, in degrees"
Maybe you could try it.