fix waypoint find function