Added waypoint upload and auto set (works)