fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 17:39:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tailsitter: use MC attitude and rate controller for FW part as well

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-07-27 00:56:37 +02:00
parent aabbdf380b
commit 7bcd33b177
4 changed files with 62 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/mc_att_control/mc_att_control_main.cpp
View File

@ -303,10 +303,7 @@ MulticopterAttitudeControl::Run()
const bool is_hovering = (_vehicle_type_rotary_wing && !_vtol_in_transition_mode);
// vehicle is a tailsitter in transition mode
const bool is_tailsitter_transition = (_vtol_tailsitter && _vtol_in_transition_mode);
bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || is_tailsitter_transition);
bool run_att_ctrl = _vehicle_control_mode.flag_control_attitude_enabled && (is_hovering || _vtol_tailsitter);
if (run_att_ctrl) {

5
src/modules/mc_rate_control/MulticopterRateControl.cpp
View File

@ -209,8 +209,9 @@ MulticopterRateControl::Run()
// run the rate controller
if (_vehicle_control_mode.flag_control_rates_enabled && !_actuators_0_circuit_breaker_enabled) {
// reset integral if disarmed
if (!_vehicle_control_mode.flag_armed || _vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
// reset integral if disarmed or we do not run the controller (VTOLs not in hover except tailsitter)
if (!_vehicle_control_mode.flag_armed || (_vehicle_status.vehicle_type != vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
&& !_vehicle_status.is_vtol_tailsitter)) {
_rate_control.resetIntegral();
}

67
src/modules/vtol_att_control/tailsitter.cpp
View File

@ -206,7 +206,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
_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
if (_v_control_mode->flag_control_altitude_enabled) {
_q_trans_start = Eulerf(0.0f, _fw_virtual_att_sp->pitch_body, yaw_sp);
} else {
_q_trans_start = Eulerf(0.0f, _mc_virtual_att_sp->pitch_body, yaw_sp);
}
// attitude during transitions are controlled by mc attitude control so rotate the desired attitude to the
// multirotor frame
@ -285,7 +291,48 @@ void Tailsitter::waiting_on_tecs()
void Tailsitter::update_fw_state()
{
VtolType::update_fw_state();
if (_flag_idle_mc) {
_flag_idle_mc = !set_idle_fw();
}
// 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));
} else {
memcpy(_v_att_sp, _mc_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));
_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));
// // 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
// _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();
// 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);
_q_trans_sp.copyTo(_v_att_sp->q_d);
}
@ -330,27 +377,27 @@ void Tailsitter::fill_actuator_outputs()
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW];
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE];
// FW thrust is allocated on mc_thrust_sp[0] for tailsitter with dynamic control allocation
_thrust_setpoint_0->xyz[2] = -fw_in[actuator_controls_s::INDEX_THROTTLE];
_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)
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
mc_out[actuator_controls_s::INDEX_YAW] = -fw_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_r.get() ;
_torque_setpoint_0->xyz[2] = -fw_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_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() ;
}
// output differential thrust for pitch if enabled
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT)) {
mc_out[actuator_controls_s::INDEX_PITCH] = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ;
_torque_setpoint_0->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ;
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ;
_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)
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::YAW_BIT)) {
mc_out[actuator_controls_s::INDEX_ROLL] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
_torque_setpoint_0->xyz[0] = fw_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_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() ;
}
} else {

2
src/modules/vtol_att_control/vtol_att_control_main.cpp
View File

@ -364,7 +364,7 @@ VtolAttitudeControl::Run()
// vehicle is in fw mode
_vtol_vehicle_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_FW;
if (fw_att_sp_updated) {
if (mc_att_sp_updated || fw_att_sp_updated) {
_vtol_type->update_fw_state();
_vehicle_attitude_sp_pub.publish(_vehicle_attitude_sp);
}