Remap angles

Not sure if this is correct or not but it certainly seems to help.
NavUtils

One round of testing then ship it! :)

More unit testing is recommended, but basic tests work.