From 92afe45f7f4b6ed4fcc9cb44685544d7581a8cea Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Thu, 24 Dec 2020 09:29:17 -0500 Subject: [PATCH] vtol_att_control: improve readability of fill_actuator_outputs() - vtol standard pass through landing gear for MC and flaps for FW --- src/modules/vtol_att_control/standard.cpp | 137 ++++++++++---------- src/modules/vtol_att_control/tailsitter.cpp | 38 +++--- src/modules/vtol_att_control/tiltrotor.cpp | 55 ++++---- 3 files changed, 110 insertions(+), 120 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index a51a2dee08..4079b517f3 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -342,78 +342,77 @@ void Standard::update_fw_state() */ void Standard::fill_actuator_outputs() { - // multirotor controls - _actuators_out_0->timestamp = hrt_absolute_time(); + auto &mc_in = _actuators_mc_in->control; + auto &fw_in = _actuators_fw_in->control; + + auto &mc_out = _actuators_out_0->control; + auto &fw_out = _actuators_out_1->control; + + const bool elevon_lock = (_params->elevons_mc_lock == 1); + + switch (_vtol_schedule.flight_mode) { + case vtol_mode::MC_MODE: + + // MC out = MC in + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL]; + mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH]; + mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW]; + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; + mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = mc_in[actuator_controls_s::INDEX_LANDING_GEAR]; + + // FW out = 0, other than roll and pitch depending on elevon lock + fw_out[actuator_controls_s::INDEX_ROLL] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_ROLL]; + fw_out[actuator_controls_s::INDEX_PITCH] = elevon_lock ? 0 : fw_in[actuator_controls_s::INDEX_PITCH]; + fw_out[actuator_controls_s::INDEX_YAW] = 0; + fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; + fw_out[actuator_controls_s::INDEX_FLAPS] = 0; + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0; + + break; + + case vtol_mode::TRANSITION_TO_FW: + + // FALLTHROUGH + case vtol_mode::TRANSITION_TO_MC: + // MC out = MC in (weighted) + 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; + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; + mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = 0; + + // FW out = FW in, with VTOL transition controlling throttle and airbrakes + 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]; + fw_out[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; + fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS]; + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output; + + break; + + case vtol_mode::FW_MODE: + // MC out = 0 + mc_out[actuator_controls_s::INDEX_ROLL] = 0; + mc_out[actuator_controls_s::INDEX_PITCH] = 0; + mc_out[actuator_controls_s::INDEX_YAW] = 0; + mc_out[actuator_controls_s::INDEX_THROTTLE] = 0; + mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = 0; + + // FW out = FW in + 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]; + fw_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE]; + fw_out[actuator_controls_s::INDEX_FLAPS] = fw_in[actuator_controls_s::INDEX_FLAPS]; + fw_out[actuator_controls_s::INDEX_AIRBRAKES] = 0; + break; + } + _actuators_out_0->timestamp_sample = _actuators_mc_in->timestamp_sample; - - // roll - _actuators_out_0->control[actuator_controls_s::INDEX_ROLL] = - _actuators_mc_in->control[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight; - // pitch - _actuators_out_0->control[actuator_controls_s::INDEX_PITCH] = - _actuators_mc_in->control[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight; - // yaw - _actuators_out_0->control[actuator_controls_s::INDEX_YAW] = - _actuators_mc_in->control[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight; - // throttle - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight; - - - // fixed wing controls - _actuators_out_1->timestamp = hrt_absolute_time(); _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; - if (_vtol_schedule.flight_mode != vtol_mode::MC_MODE) { - // roll - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - _actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; - - // pitch - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = - _actuators_fw_in->control[actuator_controls_s::INDEX_PITCH]; - // yaw - _actuators_out_1->control[actuator_controls_s::INDEX_YAW] = - _actuators_fw_in->control[actuator_controls_s::INDEX_YAW]; - - _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = _reverse_output; - - } else { - - if (_params->elevons_mc_lock) { - // zero outputs when inactive - _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; - _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f; - - } else { - // roll - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = - _actuators_fw_in->control[actuator_controls_s::INDEX_ROLL]; - - // pitch - _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] = 0.0f; - _actuators_out_1->control[actuator_controls_s::INDEX_AIRBRAKES] = 0.0f; - } - } - - // set the fixed wing throttle control - if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) { - - // take the throttle value commanded by the fw controller - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_fw_in->control[actuator_controls_s::INDEX_THROTTLE]; - - } else { - // otherwise we may be ramping up the throttle during the transition to fw mode - _actuators_out_1->control[actuator_controls_s::INDEX_THROTTLE] = _pusher_throttle; - } - - + _actuators_out_0->timestamp = _actuators_out_1->timestamp = hrt_absolute_time(); } void diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index cc264daaf8..c503df7e63 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -280,36 +280,34 @@ void Tailsitter::update_fw_state() */ void Tailsitter::fill_actuator_outputs() { - _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_1->timestamp = hrt_absolute_time(); - _actuators_out_1->timestamp_sample = _actuators_fw_in->timestamp_sample; + auto &mc_out = _actuators_out_0->control; + auto &fw_out = _actuators_out_1->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; + 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]; } else { - _actuators_out_0->control[actuator_controls_s::INDEX_THROTTLE] = - _actuators_mc_in->control[actuator_controls_s::INDEX_THROTTLE]; + mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE]; } if (_params->elevons_mc_lock && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) { - _actuators_out_1->control[actuator_controls_s::INDEX_ROLL] = 0; - _actuators_out_1->control[actuator_controls_s::INDEX_PITCH] = 0; + fw_out[actuator_controls_s::INDEX_ROLL] = 0; + fw_out[actuator_controls_s::INDEX_PITCH] = 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]; + 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]; } + + _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(); } diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index 4d19a76c6a..56255a89e6 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -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); - }