Add WAYPOINT_DEBUG to display some messages