diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 89c2d4367a..93c440e0f7 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -106,7 +106,19 @@ void Standard::update_vtol_state() float mc_weight = _mc_roll_weight; float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; - if (!_attc->is_fixed_wing_requested()) { + if (_vtol_vehicle_status->vtol_transition_failsafe) { + // Failsafe event, engage mc motors immediately + _vtol_schedule.flight_mode = vtol_mode::MC_MODE; + _pusher_throttle = 0.0f; + _reverse_output = 0.0f; + + //reset failsafe when FW is no longer requested + if (!_attc->is_fixed_wing_requested()) { + _vtol_vehicle_status->vtol_transition_failsafe = false; + } + + + } else if (!_attc->is_fixed_wing_requested()) { // the transition to fw mode switch is off if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) { @@ -117,21 +129,10 @@ void Standard::update_vtol_state() _reverse_output = 0.0f; } else if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { - // transition to mc mode - if (_vtol_vehicle_status->vtol_transition_failsafe) { - // Failsafe event, engage mc motors immediately - _vtol_schedule.flight_mode = vtol_mode::MC_MODE; - _pusher_throttle = 0.0f; - _reverse_output = 0.0f; - - - } else { - // Regular backtransition - _vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC; - _vtol_schedule.transition_start = hrt_absolute_time(); - _reverse_output = _params_standard.reverse_output; - - } + // Regular backtransition + _vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_MC; + _vtol_schedule.transition_start = hrt_absolute_time(); + _reverse_output = _params_standard.reverse_output; } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { // failsafe back to mc mode @@ -162,7 +163,7 @@ void Standard::update_vtol_state() if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE || _vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { // start transition to fw mode /* NOTE: The failsafe transition to fixed-wing was removed because it can result in an - * unsafe flying state. */ + * unsafe flying state. */ _vtol_schedule.flight_mode = vtol_mode::TRANSITION_TO_FW; _vtol_schedule.transition_start = hrt_absolute_time(); diff --git a/src/modules/vtol_att_control/vtol_att_control_main.cpp b/src/modules/vtol_att_control/vtol_att_control_main.cpp index e5e975a3e8..0e7fd8cfe6 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.cpp +++ b/src/modules/vtol_att_control/vtol_att_control_main.cpp @@ -207,27 +207,14 @@ VtolAttitudeControl::is_fixed_wing_requested() to_fw = (_transition_command == vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW); } - // handle abort request - if (_abort_front_transition) { - if (to_fw) { - to_fw = false; - - } else { - // the state changed to mc mode, reset the abort request - _abort_front_transition = false; - _vtol_vehicle_status.vtol_transition_failsafe = false; - } - } - return to_fw; } void VtolAttitudeControl::abort_front_transition(const char *reason) { - if (!_abort_front_transition) { + if (!_vtol_vehicle_status.vtol_transition_failsafe) { mavlink_log_critical(&_mavlink_log_pub, "Abort: %s", reason); - _abort_front_transition = true; _vtol_vehicle_status.vtol_transition_failsafe = true; } } diff --git a/src/modules/vtol_att_control/vtol_att_control_main.h b/src/modules/vtol_att_control/vtol_att_control_main.h index 3c6b35f6c8..29a872453e 100644 --- a/src/modules/vtol_att_control/vtol_att_control_main.h +++ b/src/modules/vtol_att_control/vtol_att_control_main.h @@ -222,7 +222,6 @@ private: * for fixed wings we want to have an idle speed of zero since we do not want * to waste energy when gliding. */ int _transition_command{vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC}; - bool _abort_front_transition{false}; VtolType *_vtol_type{nullptr}; // base class for different vtol types