Updates to fixed wing navigation code. No multicopter navigation yet.