Add debug for navigation, make sure that destination is in range before calling waypoint triggers