Save the distance and heading to the next waypoint