Added check if autopilot_motors_on do determine if you want to switch to AP_MODE_FAILSAFE