diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 525a986eb5..a35a448fe5 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -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; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index f11a5204d7..7e118428c1 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -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: diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index f815b53e83..52bd0b48c8 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -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::Vectorxyz[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