Fix for rejected waypoint