General code tidy and allowing for more frequent waypoints.