Improvements to addWaypoint and taskAmbush