[c12469] Improve Waypoint Management