From aa87a342e6f6f52ee641675a12d6a2053a317f13 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 9 Aug 2022 10:12:08 +0200 Subject: [PATCH] use FW attitude controller in FW but always MC rate controller Signed-off-by: Silvan Fuhrer --- .../FixedwingAttitudeControl.cpp | 10 +++ .../mc_att_control/mc_att_control_main.cpp | 7 +- src/modules/vtol_att_control/tailsitter.cpp | 68 +++++++++++-------- 3 files changed, 57 insertions(+), 28 deletions(-) diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index fdf81954af..6ace56fbdc 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -151,6 +151,7 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) _att_sp.yaw_body = yaw_body; // yaw is not controlled, so set setpoint to current yaw _att_sp.thrust_body[0] = math::constrain(_manual_control_setpoint.z, 0.0f, 1.0f); + // TODO rotate thrust prior to publication for tailsitter? Quatf q(Eulerf(_att_sp.roll_body, _att_sp.pitch_body, _att_sp.yaw_body)); q.copyTo(_att_sp.q_d); @@ -602,6 +603,15 @@ void FixedwingAttitudeControl::Run() _rates_sp.timestamp = hrt_absolute_time(); + // rotate output of FW attitude controller to body frame + if (_vehicle_status.is_vtol_tailsitter) { + const float tmp = _rates_sp.roll; + _rates_sp.roll = _rates_sp.yaw; + _rates_sp.yaw = -tmp; + _rates_sp.thrust_body[2] = -_rates_sp.thrust_body[0]; + _rates_sp.thrust_body[0] = 0.f; + } + _rate_sp_pub.publish(_rates_sp); } else { diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 94dc87bb22..39675fea2c 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -303,7 +303,12 @@ MulticopterAttitudeControl::Run() const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode); - bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || _vtol_tailsitter); + const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode); + + const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing + + bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition + || run_mc_att_c_in_fw_tailsitter); if (run_att_ctrl) { diff --git a/src/modules/vtol_att_control/tailsitter.cpp b/src/modules/vtol_att_control/tailsitter.cpp index 7f0e188d49..6a48c0adfd 100644 --- a/src/modules/vtol_att_control/tailsitter.cpp +++ b/src/modules/vtol_att_control/tailsitter.cpp @@ -207,11 +207,13 @@ void Tailsitter::update_transition_state() // the intial attitude setpoint for a backtransition is a combination of the current fw pitch setpoint, // the yaw setpoint and zero roll since we want wings level transition - if (_v_control_mode->flag_control_altitude_enabled) { - _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); + const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing + + if (run_mc_att_c_in_fw_tailsitter) { + _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, yaw_sp); } else { - _q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, yaw_sp); + _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); } // attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the @@ -295,22 +297,36 @@ void Tailsitter::update_fw_state() _flag_idle_mc = !set_idle_fw(); } + const bool run_mc_att_c_in_fw_tailsitter = false; // TODO: just for testing + // copy virtual attitude setpoint to real attitude setpoint - if (_v_control_mode->flag_control_altitude_enabled) { - memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + if (run_mc_att_c_in_fw_tailsitter) { + memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } else { - memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); + memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s)); } check_quadchute_condition(); - // calculate rotation axis for transition. - _q_trans_start = Quatf(_v_att->q); - Vector3f z = -_q_trans_start.dcm_z(); - _trans_rot_axis = z.cross(Vector3f(0, 0, -1)); + // printf("""""""""""""\n"); + // printf("roll sp: %f\n", (double)_v_att_sp->roll_body); + // printf("pitch sp: (raw) %f\n", (double)_v_att_sp->pitch_body); + // printf("yaw_sp: %f\n", (double)_v_att_sp->yaw_body); - _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, M_PI_2_F)); + // // rotate setpoint 90° down + // _v_att_sp->roll_body = _v_att_sp->roll_body; + // _v_att_sp->pitch_body = _v_att_sp->pitch_body - M_PI_2_F; + // _v_att_sp->yaw_body = _v_att_sp->yaw_body; + // Quatf q(Eulerf(_v_att_sp->roll_body, _v_att_sp->pitch_body, _v_att_sp->yaw_body)); + // q.copyTo(_v_att_sp->q_d); + + // // calculate rotation axis for transition. + // _q_trans_start = Quatf(_v_att->q); + // Vector3f z = -_q_trans_start.dcm_z(); + // _trans_rot_axis = z.cross(Vector3f(0, 0, -1)); + + // _q_trans_sp = Quatf(AxisAnglef(_trans_rot_axis, M_PI_2_F)); // // as heading setpoint we choose the heading given by the direction the vehicle points // float yaw_sp = atan2f(z(1), z(0)); @@ -320,19 +336,17 @@ void Tailsitter::update_fw_state() // _q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp); - const Eulerf euler_sp(_q_trans_sp); - _v_att_sp->roll_body = euler_sp.phi(); - _v_att_sp->pitch_body = euler_sp.theta(); - _v_att_sp->yaw_body = euler_sp.psi(); + // const Eulerf euler_sp(_q_trans_sp); + // _v_att_sp->roll_body = euler_sp.phi(); + // _v_att_sp->pitch_body = euler_sp.theta(); + // _v_att_sp->yaw_body = euler_sp.psi(); - // printf("""""""""""""\n"); - // printf("phi: %f\n", (double)euler_sp.phi()); - // printf("theta: %f\n", (double)euler_sp.theta()); - // printf("psi: %f\n", (double)euler_sp.psi()); - // printf("xxxxx: %\n", (double)xxxxx); + // printf("roll sp: %f\n", (double)_v_att_sp->roll_body); + // printf("pitch sp: (rotated) %f\n", (double)_v_att_sp->pitch_body); + // printf("yaw_sp: %f\n", (double)_v_att_sp->yaw_body); - _q_trans_sp.copyTo(_v_att_sp->q_d); + // _q_trans_sp.copyTo(_v_att_sp->q_d); } @@ -382,10 +396,10 @@ void Tailsitter::fill_actuator_outputs() // FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation _thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE]; - // output differential thrust for roll if enabled (to achieve roll contorl in FW we need controller yaw output) + // output differential thrust for roll (FW frame) if enabled (to achieve roll contorl in FW we need controller yaw output) if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::ROLL_BIT)) { - mc_out[actuator_controls_s::INDEX_YAW] = -mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get() ; - _torque_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get() ; + mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_r.get() ; + _torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_r.get() ; } // output differential thrust for pitch if enabled @@ -394,10 +408,10 @@ void Tailsitter::fill_actuator_outputs() _torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ; } - // output differential thrust for yaw if enabled (to achieve yaw contorl in FW we need controller roll output) + // output differential thrust for yaw (FW frame) if enabled (to achieve yaw contorl in FW we need controller roll output) if (_param_vt_fw_difthr_en.get() & static_cast(VtFwDifthrEnBits::YAW_BIT)) { - mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; - _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ; + mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_y.get() ; + _torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_y.get() ; } } else {