fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-30 06:03:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

VTOL: remove virtual actuator_controls and instead use virtual torque/thrust topics

Browse Source 
MC/FW rate controller and auto tuner: remove actuator_controls

AirshipAttControl: remove actuator_controls

MulticopterLandDetector: remove actuator_controls

mavlink streams vfr_hud and high_latency2: remove actuator_controls

RoverPositionController: remove actuator_controls

UUVAttitudeController: remove actuator_controls

battery: use length of thrust_setpoint for throttle compensation

VehicleMagnetometer: use length of thrust_setpoint for throttle compensation

Signed-off-by: Silvan Fuhrer
This commit is contained in:
Silvan Fuhrer
2023-03-16 11:55:45 +01:00
committed by Beat Küng
parent bcd6e7adee
commit b16f16598b
47 changed files with 332 additions and 549 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vtol_att_control/tiltrotor.cpp
+18 -43
View File
@@ -374,92 +374,67 @@ void Tiltrotor::waiting_on_tecs()
_v_att_sp->thrust_body[0] = _thrust_transition;
}
/**
* Write data to actuator output topic.
*/
void Tiltrotor::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;
_torque_setpoint_0->timestamp_sample = _vehicle_torque_setpoint_virtual_mc->timestamp_sample;
_torque_setpoint_0->xyz[0] = 0.f;
_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->timestamp_sample = _vehicle_torque_setpoint_virtual_fw->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->timestamp_sample = _vehicle_thrust_setpoint_virtual_mc->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->timestamp_sample = _vehicle_thrust_setpoint_virtual_fw->timestamp_sample;
_thrust_setpoint_1->xyz[0] = 0.f;
_thrust_setpoint_1->xyz[1] = 0.f;
_thrust_setpoint_1->xyz[2] = 0.f;
// Multirotor output
mc_out[actuator_controls_s::INDEX_ROLL] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
mc_out[actuator_controls_s::INDEX_PITCH] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
mc_out[actuator_controls_s::INDEX_YAW] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_torque_setpoint_0->xyz[0] = mc_in[actuator_controls_s::INDEX_ROLL] * _mc_roll_weight;
_torque_setpoint_0->xyz[1] = mc_in[actuator_controls_s::INDEX_PITCH] * _mc_pitch_weight;
_torque_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_YAW] * _mc_yaw_weight;
_torque_setpoint_0->xyz[0] = _vehicle_torque_setpoint_virtual_mc->xyz[0] * _mc_roll_weight;
_torque_setpoint_0->xyz[1] = _vehicle_torque_setpoint_virtual_mc->xyz[1] * _mc_pitch_weight;
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_mc->xyz[2] * _mc_yaw_weight;
// Special case tiltrotor: instead of passing a 3D thrust vector (that would mostly have a x-component in FW, and z in MC),
// pass the vector magnitude as z-component, plus the collective tilt. Passing 3D thrust plus tilt is not feasible as they
// pass the vector magnitude and collective tilt separately. MC also needs collective thrust on z.
// Passing 3D thrust plus tilt is not feasible as they
// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
// by the allocator directly.
float collective_thrust_normalized_setpoint = 0.f;
if (_vtol_mode == vtol_mode::FW_MODE) {
_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
collective_thrust_normalized_setpoint = fw_in[actuator_controls_s::INDEX_THROTTLE];
collective_thrust_normalized_setpoint = _vehicle_thrust_setpoint_virtual_fw->xyz[0];
_thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint;
/* allow differential thrust if enabled */
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[2] = fw_in[actuator_controls_s::INDEX_YAW] * _param_vt_fw_difthr_s_y.get() ;
_torque_setpoint_0->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2] * _param_vt_fw_difthr_s_y.get() ;
}
} else {
_thrust_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
collective_thrust_normalized_setpoint = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
collective_thrust_normalized_setpoint = -_vehicle_thrust_setpoint_virtual_mc->xyz[2] * _mc_throttle_weight;
_thrust_setpoint_0->xyz[2] = -collective_thrust_normalized_setpoint;
}
// Fixed wing output
if (_param_vt_elev_mc_lock.get() && _vtol_mode == vtol_mode::MC_MODE) {
fw_out[actuator_controls_s::INDEX_ROLL] = 0;
fw_out[actuator_controls_s::INDEX_PITCH] = 0;
fw_out[actuator_controls_s::INDEX_YAW] = 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];
fw_out[actuator_controls_s::INDEX_YAW] = fw_in[actuator_controls_s::INDEX_YAW];
_torque_setpoint_1->xyz[0] = fw_in[actuator_controls_s::INDEX_ROLL];
_torque_setpoint_1->xyz[1] = fw_in[actuator_controls_s::INDEX_PITCH];
_torque_setpoint_1->xyz[2] = fw_in[actuator_controls_s::INDEX_YAW];
if (!_param_vt_elev_mc_lock.get() || _vtol_mode != vtol_mode::MC_MODE) {
_torque_setpoint_1->xyz[0] = _vehicle_torque_setpoint_virtual_fw->xyz[0];
_torque_setpoint_1->xyz[1] = _vehicle_torque_setpoint_virtual_fw->xyz[1];
_torque_setpoint_1->xyz[2] = _vehicle_torque_setpoint_virtual_fw->xyz[2];
}
_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();
// publish tiltrotor extra controls
tiltrotor_extra_controls_s tiltrotor_extra_controls = {};
tiltrotor_extra_controls.collective_tilt_normalized_setpoint = _tilt_control;