fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-23 04:33:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

vtol_att_control: improve readability of fill_actuator_outputs()

Browse Source 
- vtol standard pass through landing gear for MC and flaps for FW
This commit is contained in:
Daniel Agar
2020-12-24 09:29:17 -05:00
committed by GitHub
parent 9849ff4a9c
commit 92afe45f7f
3 changed files with 110 additions and 120 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/tiltrotor.cpp
+24 -31
View File
@@ -388,8 +388,6 @@ void Tiltrotor::update_transition_state()
_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
_mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
}
void Tiltrotor::waiting_on_tecs()
@@ -403,51 +401,47 @@ 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;
auto &mc_in = _actuators_mc_in->control;
auto &fw_in = _actuators_fw_in->control;
_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;
auto &mc_out = _actuators_out_0->control;
auto &fw_out = _actuators_out_1->control;
// Multirotor output
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE];
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */
if (_params->diff_thrust == 1) {
_actuators_out_0->control[actuator_controls_s::INDEX_ROLL] =
_actuators_fw_in->control[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _params->diff_thrust_scale;
}
} else {
_actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] =
_actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[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[4] = _tilt_control;
fw_out[4] = _tilt_control;
if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_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;
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
fw_out[actuator_controls_s::INDEX_YAW] = 0;
} 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];
fw_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_ROLL];
fw_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH];
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
}
_actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time();
}
/*
@@ -461,5 +455,4 @@ float Tiltrotor::thrust_compensation_for_tilt()
// increase vertical thrust by 1/cos(tilt), limmit to [-1,0]
return math::constrain(_v_att_sp->thrust_body[2] / cosf(compensated_tilt * M_PI_2_F), -1.0f, 0.0f);
}