diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index d95e38c1ad..c2b721fe05 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -314,11 +314,6 @@ void Standard::update_mc_state() { VtolType::update_mc_state(); - // enable MC motors here in case we transitioned directly to MC mode - if (_motor_state != ENABLED) { - _motor_state = set_motor_state(_motor_state, ENABLED); - } - // if the thrust scale param is zero or the drone is on manual mode, // then the pusher-for-pitch strategy is disabled and we can return if (_params_standard.forward_thrust_scale < FLT_EPSILON || @@ -394,11 +389,6 @@ void Standard::update_mc_state() void Standard::update_fw_state() { VtolType::update_fw_state(); - - // stop MC motors in FW mode - if (_motor_state != DISABLED) { - _motor_state = VtolType::set_motor_state(_motor_state, DISABLED); - } } /** diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 40e5eeab82..f765c424fb 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -257,20 +257,11 @@ void Tailsitter::waiting_on_tecs() void Tailsitter::update_mc_state() { VtolType::update_mc_state(); - - // set idle speed for rotary wing mode - if (!flag_idle_mc) { - flag_idle_mc = set_idle_mc(); - } } void Tailsitter::update_fw_state() { VtolType::update_fw_state(); - - if (flag_idle_mc) { - flag_idle_mc = !set_idle_fw(); - } } /** diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 8ac14b63c5..17dbb0c359 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -216,16 +216,6 @@ void Tiltrotor::update_mc_state() // make sure motors are not tilted _tilt_control = _params_tiltrotor.tilt_mc; - - // enable rear motors - if (_motor_state != ENABLED) { - _motor_state = set_motor_state(_motor_state, ENABLED); - } - - // set idle speed for rotary wing mode - if (!flag_idle_mc) { - flag_idle_mc = set_idle_mc(); - } } void Tiltrotor::update_fw_state() @@ -234,16 +224,6 @@ void Tiltrotor::update_fw_state() // make sure motors are tilted forward _tilt_control = _params_tiltrotor.tilt_fw; - - // disable rear motors - if (_motor_state != DISABLED) { - _motor_state = set_motor_state(_motor_state, DISABLED); - } - - // adjust idle for fixed wing flight - if (flag_idle_mc) { - flag_idle_mc = !set_idle_fw(); - } } void Tiltrotor::update_transition_state() diff --git a/src/modules/vtol_att_control/vtol_type.cpp b/src/modules/vtol_att_control/vtol_type.cpp index fd5c214f26..4c2fe698b6 100644 --- a/src/modules/vtol_att_control/vtol_type.cpp +++ b/src/modules/vtol_att_control/vtol_type.cpp @@ -115,6 +115,14 @@ bool VtolType::init() void VtolType::update_mc_state() { + if (!flag_idle_mc) { + flag_idle_mc = set_idle_mc(); + } + + if (_motor_state != ENABLED) { + _motor_state = VtolType::set_motor_state(_motor_state, ENABLED); + } + // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); @@ -143,6 +151,14 @@ void VtolType::update_mc_state() void VtolType::update_fw_state() { + if (flag_idle_mc) { + flag_idle_mc = !set_idle_fw(); + } + + if (_motor_state != DISABLED) { + _motor_state = VtolType::set_motor_state(_motor_state, DISABLED); + } + // copy virtual attitude setpoint to real attitude setpoint memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); _mc_roll_weight = 0.0f;