Added GPS update code, need to test on physical hardware and debug if needed.