Add GPS/location test routines
Fix bug in goal adjustment