From 8dd76050e0c6d50d818decbb4c5d0cd4138bf3eb Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Mon, 26 Apr 2021 14:33:17 +0300 Subject: [PATCH] vtol: take fixed wing attitude setpoint during transition if altitude is not controlled - required as there is no flightask running if altitude is not controlled Signed-off-by: RomanBapst --- src/modules/vtol_att_control/standard.cpp | 26 ++++++---------- src/modules/vtol_att_control/tiltrotor.cpp | 36 +++++++--------------- 2 files changed, 21 insertions(+), 41 deletions(-) diff --git a/src/modules/vtol_att_control/standard.cpp b/src/modules/vtol_att_control/standard.cpp index 9579fefa73..2e96304024 100644 --- a/src/modules/vtol_att_control/standard.cpp +++ b/src/modules/vtol_att_control/standard.cpp @@ -230,8 +230,16 @@ void Standard::update_transition_state() VtolType::update_transition_state(); - // copy virtual attitude setpoint to real attitude setpoint - memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // we get attitude setpoint from a multirotor flighttask if altitude is controlled. + // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. + if (_v_control_mode->flag_control_climb_rate_enabled) { + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + + } else { + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + } if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_FW) { if (_params_standard.pusher_ramp_dt <= 0.0f) { @@ -263,13 +271,6 @@ void Standard::update_transition_state() // ramp up FW_PSP_OFF _v_att_sp->pitch_body = _params_standard.pitch_setpoint_offset * (1.0f - mc_weight); - _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); @@ -283,18 +284,11 @@ void Standard::update_transition_state() } else if (_vtol_schedule.flight_mode == vtol_mode::TRANSITION_TO_MC) { - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; - if (_v_control_mode->flag_control_climb_rate_enabled) { // control backtransition deceleration using pitch. _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 2734b05018..ff90df1f67 100644 --- a/src/modules/vtol_att_control/tiltrotor.cpp +++ b/src/modules/vtol_att_control/tiltrotor.cpp @@ -289,10 +289,18 @@ void Tiltrotor::update_transition_state() { VtolType::update_transition_state(); - // copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp) - memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + // we get attitude setpoint from a multirotor flighttask if altitude is controlled. + // in any other case the fixed wing attitude controller publishes attitude setpoint from manual stick input. + if (_v_control_mode->flag_control_climb_rate_enabled) { + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + _thrust_transition = -_mc_virtual_att_sp->thrust_body[2]; - _v_att_sp->roll_body = _fw_virtual_att_sp->roll_body; + } else { + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + _v_att_sp->thrust_body[2] = -_fw_virtual_att_sp->thrust_body[0]; + _thrust_transition = _fw_virtual_att_sp->thrust_body[0]; + } float time_since_trans_start = (float)(hrt_absolute_time() - _vtol_schedule.transition_start) * 1e-6f; @@ -339,13 +347,6 @@ void Tiltrotor::update_transition_state() _mc_yaw_weight = _mc_roll_weight; } - _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 + @@ -359,22 +360,12 @@ void Tiltrotor::update_transition_state() int ramp_down_value = (1.0f - time_since_trans_start / _params_tiltrotor.front_trans_dur_p2) * (PWM_DEFAULT_MAX - PWM_DEFAULT_MIN) + PWM_DEFAULT_MIN; - set_alternate_motor_state(motor_state::VALUE, ramp_down_value); - - _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); - // set idle speed for rotary wing mode if (!_flag_idle_mc) { _flag_idle_mc = set_idle_mc(); @@ -405,11 +396,6 @@ 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));