NavCheckWaypoint function added