Unit tests

Filled out some unit tests, GPS tested