Decouple navigation from mavlink