Waypoints: Fix command orientation issues

[zachgoins]

Figure out exactly what commands do what in the sub waypoint handler and convert them to work smoothly on the boat.

[fnivek]

I'm pretty sure everything works well. Reopen this if you think otherwise.