fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-08 16:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

Tailsitter: only support single matrix and mc rate controller, and CA only

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-08-09 15:22:30 +02:00
parent 9364792da1
commit f624b963e2
2 changed files with 6 additions and 66 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp
+1 -1
View File
@@ -61,7 +61,7 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi
const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration);
// Control Surfaces
configuration.selected_matrix = 1;
configuration.selected_matrix = 0; // for tailsitter we only use one matrix, as opposed to the other VTOL types
_first_control_surface_idx = configuration.num_actuators_matrix[configuration.selected_matrix];
const bool surfaces_added_successfully = _control_surfaces.addActuators(configuration);
src/modules/vtol_att_control/tailsitter.cpp
+5 -65
View File
@@ -356,10 +356,8 @@ void Tailsitter::update_fw_state()
void Tailsitter::fill_actuator_outputs()
{
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;
_torque_setpoint_0->timestamp = hrt_absolute_time();
_torque_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
@@ -367,70 +365,25 @@ void Tailsitter::fill_actuator_outputs()
_torque_setpoint_0->xyz[1] = 0.f;
_torque_setpoint_0->xyz[2] = 0.f;
_torque_setpoint_1->timestamp = hrt_absolute_time();
_torque_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_torque_setpoint_1->xyz[0] = 0.f;
_torque_setpoint_1->xyz[1] = 0.f;
_torque_setpoint_1->xyz[2] = 0.f;
_thrust_setpoint_0->timestamp = hrt_absolute_time();
_thrust_setpoint_0->timestamp_sample = _actuators_mc_in->timestamp_sample;
_thrust_setpoint_0->xyz[0] = 0.f;
_thrust_setpoint_0->xyz[1] = 0.f;
_thrust_setpoint_0->xyz[2] = 0.f;
_thrust_setpoint_1->timestamp = hrt_absolute_time();
_thrust_setpoint_1->timestamp_sample = _actuators_fw_in->timestamp_sample;
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;
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];
if (_vtol_schedule.flight_mode == vtol_mode::FW_MODE) {
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] = -mc_in[actuator_controls_s::INDEX_THROTTLE];
// 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<int32_t>(VtFwDifthrEnBits::ROLL_BIT)) {
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
if (_param_vt_fw_difthr_en.get() & static_cast<int32_t>(VtFwDifthrEnBits::PITCH_BIT)) {
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 (FW frame) 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] = 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();
}
_torque_setpoint_1->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_r.get();
_torque_setpoint_1->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get();
_torque_setpoint_1->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_y.get();
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_r.get();
_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _param_vt_fw_difthr_s_p.get() ;
_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _param_vt_fw_difthr_s_y.get();
} else {
_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW];
_torque_setpoint_1->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_1->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW];
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE];
_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE];
}
_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE];
// Landing Gear
if (_vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_DOWN;
@@ -439,19 +392,6 @@ void Tailsitter::fill_actuator_outputs()
mc_out[actuator_controls_s::INDEX_LANDING_GEAR] = landing_gear_s::GEAR_UP;
}
if (_param_vt_mc_cs_lock.get() && _vtol_schedule.flight_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
} else {
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];
// _torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH];
// _torque_setpoint_1->xyz[2] = -fw_in[actuator_controls_s::INDEX_ROLL];
}
_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();
}