From 706f806943edb56030490832dd8142f4a170f053 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 19 Nov 2020 19:53:14 +0100 Subject: [PATCH] VTOL: standard VTOL and Tiltrotor manual MC throttle during transition This sets the throttle of the Multicopter actuators to the throttle stick position while in Stabilized, Acro or Manual mode. Signed-off-by: Silvan Fuhrer --- src/modules/vtol_att_control/standard.cpp | 10 ++++++++++ src/modules/vtol_att_control/tiltrotor.cpp | 15 +++++++++++++++ 2 files changed, 25 insertions(+) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 6dc8d13003..89c2d4367a 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -267,6 +267,11 @@ void Standard::update_transition_state() _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); @@ -287,6 +292,11 @@ void Standard::update_transition_state() _v_att_sp->pitch_body = update_and_get_backtransition_pitch_sp(); } + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); q_sp.copyTo(_v_att_sp->q_d); diff --git a/src/modules/vtol_att_control/tiltrotor.cpp b/src/modules/vtol_att_control/tiltrotor.cpp index c9c5eb5931..672db19c62 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -322,6 +322,11 @@ void Tiltrotor::update_transition_state() _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_FRONT_P2) { // the plane is ready to go into fixed wing mode, tilt the rotors forward completely _tilt_control = _params_tiltrotor.tilt_transition + @@ -341,6 +346,11 @@ void Tiltrotor::update_transition_state() _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } + } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_BACK) { // turn on all MC motors set_all_motor_state(motor_state::ENABLED); @@ -376,6 +386,11 @@ void Tiltrotor::update_transition_state() // slowly ramp up throttle to avoid step inputs _mc_throttle_weight = (time_since_trans_start - 1.0f) / 1.0f; } + + // in stabilized, acro or manual mode, set the MC thrust to the throttle stick position (coming from the FW attitude setpoint) + if (!_v_control_mode->flag_control_climb_rate_enabled) { + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } } const Quatf q_sp(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body));