More gps testing done, waypoint incrementing not working