Initial goal_wp lat/long set to original loc
Mover in out tester finished.
Merge branch 'master' of https://github.com/cunnia3/PiDeCAF