diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index a919e99da6..272ad4cb44 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -281,12 +281,6 @@ void Tailsitter::fill_actuator_outputs() _thrust_setpoint_0->xyz[2] = -_vehicle_thrust_setpoint_virtual_fw->xyz[0]; - // for the short period after switching to FW where there is no thrust published yet from the FW controller, - // keep publishing the last MC thrust to keep the motors running - if (hrt_elapsed_time(&_trans_finished_ts) < 50_ms) { - _thrust_setpoint_0->xyz[2] = _last_thr_in_mc; - } - /* allow differential thrust if enabled */ if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { _torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0] * _param_vt_fw_difthr_s_y.get(); @@ -300,6 +294,15 @@ void Tailsitter::fill_actuator_outputs() _torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_r.get(); } + // for the short period after switching to FW where there is no thrust published yet from the FW controller, + // keep publishing the last MC thrust to keep the motors running + if (hrt_elapsed_time(&_trans_finished_ts) < 50_ms) { + _thrust_setpoint_0->xyz[2] = _last_thr_in_mc; + _torque_setpoint_0->xyz[0] = 0.f; + _torque_setpoint_0->xyz[1] = 0.f; + _torque_setpoint_0->xyz[2] = 0.f; + } + } else { _thrust_setpoint_0->xyz[2] = _vehicle_thrust_setpoint_virtual_mc->xyz[2];