Integrate the NHADRIAN's idea of Failsafe

Non-tested.
nav version already has rth, hold, landing function. Integrate with
those function while failsafe happened.