diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 04a58a46de..6d51be4d73 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -334,15 +334,16 @@ void Tiltrotor::waiting_on_tecs() */ void Tiltrotor::fill_actuator_outputs() { + // Multirotor output _actuators_out_0->timestamp = hrt_absolute_time(); _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] - * _mc_roll_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = + _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * - _mc_yaw_weight; + _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = + _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; if (_vtol_schedule.flight_mode == FW_MODE) { _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = @@ -359,14 +360,23 @@ void Tiltrotor::fill_actuator_outputs() _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; } + // Fixed wing output _actuators_out_1->timestamp = hrt_absolute_time(); _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - (_actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]); - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; // yaw _actuators_out_1->control[4] = _tilt_control; + + if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == MC_MODE) { + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0.0f; + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = 0.0f; + + } else { + _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = + -_actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; + _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = + _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]; + _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = + _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; + } }