diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index e54e0a5bb5..1184d594eb 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -146,9 +146,17 @@ void Standard::update_vtol_state() } else if (_vtol_schedule.flight_mode == FW_MODE) { // transition to mc mode - _vtol_schedule.flight_mode = TRANSITION_TO_MC; - _flag_enable_mc_motors = true; - _vtol_schedule.transition_start = hrt_absolute_time(); + if (_vtol_vehicle_status->vtol_transition_failsafe == true){ + // Failsafe event, engage mc motors immediately + _vtol_schedule.flight_mode = MC_MODE; + _flag_enable_mc_motors = true; + + } else { + // Regular backtransition + _vtol_schedule.flight_mode = TRANSITION_TO_MC; + _flag_enable_mc_motors = true; + _vtol_schedule.transition_start = hrt_absolute_time(); + } } else if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) { // failsafe back to mc mode