AP_HAL_PX4: failsafe code cleanup, init fix