Fixed #242; Added unit tests
Merge pull request #14 from BrianAdams/feature/unittests

Tested on a 2.6 ROV. Everything works correctly. Noticed some lag in the heading hold response but I don't see any connection to this change.