Finished first version of doGps
mostly moved constants around