fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Control Allocation: Tiltrotor: pass only thrust magnitude instead of 3D thrust to allocator

Browse Source 
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
can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
by the allocator directly.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2022-05-05 16:32:38 +02:00
parent a71d101869
commit 46179586fb
5 changed files with 35 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
View File

@ -212,6 +212,17 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry
effectiveness(j, i + actuator_start_index) = moment(j);
effectiveness(j + 3, i + actuator_start_index) = thrust(j);
}
if (geometry.three_dimensional_thrust_disabled) {
// 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
// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
// by the allocator directly.
effectiveness(0 + 3, i + actuator_start_index) = 0.f;
effectiveness(1 + 3, i + actuator_start_index) = 0.f;
effectiveness(2 + 3, i + actuator_start_index) = ct;
}
}
return num_actuators;

3
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp
View File

@ -75,6 +75,7 @@ public:
int num_rotors{0};
bool propeller_torque_disabled{false};
bool propeller_torque_disabled_non_upwards{false}; ///< keeps propeller torque enabled for upward facing motors
bool three_dimensional_thrust_disabled{false}; ///< for handling of tiltrotor VTOL, as they pass in 1D thrust and collective tilt
};
ActuatorEffectivenessRotors(ModuleParams *parent, AxisConfiguration axis_config = AxisConfiguration::Configurable,
@ -120,6 +121,8 @@ public:
void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; }
void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; }
uint32_t getUpwardsMotors() const;
private:

13
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp
View File

@ -61,6 +61,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config
// MC motors
configuration.selected_matrix = 0;
_mc_rotors.enablePropellerTorque(!_tilts.hasYawControl());
_mc_rotors.enableThreeDimensionalThrust(false);
// Update matrix with tilts in vertical position when update is triggered by a manual
// configuration (parameter) change. This is to make sure the normalization
@ -130,6 +131,18 @@ void ActuatorEffectivenessTiltrotorVTOL::updateSetpoint(const matrix::Vector<flo
}
}
}
// in FW directly use throttle sp
if (_flight_phase == FlightPhase::FORWARD_FLIGHT) {
actuator_controls_s actuator_controls_0;
if (_actuator_controls_0_sub.copy(&actuator_controls_0)) {
for (int i = 0; i < _first_tilt_idx; ++i) {
actuator_sp(i) = actuator_controls_0.control[actuator_controls_s::INDEX_THROTTLE];
}
}
}
}
}

1
src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp
View File

@ -95,4 +95,5 @@ protected:
float _last_tilt_control{NAN};
uORB::Subscription _actuator_controls_1_sub{ORB_ID(actuator_controls_1)};
uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
};

11
src/modules/vtol_att_control/tiltrotor.cpp
View File

@ -514,8 +514,11 @@ void Tiltrotor::fill_actuator_outputs()
// for the legacy mixing system pubish FW throttle on the MC output
mc_out[actuator_controls_s::INDEX_THROTTLE] = fw_in[actuator_controls_s::INDEX_THROTTLE];
// FW thrust is allocated on mc_thrust_sp[0] for tiltrotor with dynamic control allocation
_thrust_setpoint_0->xyz[0] = fw_in[actuator_controls_s::INDEX_THROTTLE];
// 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
// can't be allocated independently, and with the current controller it's not possible to have collective tilt calculated
// by the allocator directly.
_thrust_setpoint_0->xyz[2] = fw_in[actuator_controls_s::INDEX_THROTTLE];
/* allow differential thrust if enabled */
if (_params->diff_thrust == 1) {
@ -525,9 +528,9 @@ void Tiltrotor::fill_actuator_outputs()
} else {
// see comment above for passing magnitude of thrust, not 3D thrust
mc_out[actuator_controls_s::INDEX_THROTTLE] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
_thrust_setpoint_0->xyz[2] = -mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
_thrust_setpoint_0->xyz[0] = -_thrust_setpoint_0->xyz[2] * sinf(_tilt_control * M_PI_2_F);
_thrust_setpoint_0->xyz[2] = mc_in[actuator_controls_s::INDEX_THROTTLE] * _mc_throttle_weight;
}
// Landing gear