Add waypoint code