[c2387] Improve Waypoint Management