Got GPS and velocity to work