Fixed unused waypoint variable, updated