Added basic test code for GPS unit, WIP