Finally fixed the waypoint navigation nonsense.