Waypoint navigation fixes