From 46179586fbf1b3d7e9b5aba5db88646737bc6e90 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Thu, 5 May 2022 16:32:38 +0200 Subject: [PATCH] Control Allocation: Tiltrotor: pass only thrust magnitude instead of 3D thrust to allocator 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 --- .../ActuatorEffectivenessRotors.cpp | 11 +++++++++++ .../ActuatorEffectivenessRotors.hpp | 3 +++ .../ActuatorEffectivenessTiltrotorVTOL.cpp | 13 +++++++++++++ .../ActuatorEffectivenessTiltrotorVTOL.hpp | 1 + src/modules/vtol_att_control/tiltrotor.cpp | 11 +++++++---- 5 files changed, 35 insertions(+), 4 deletions(-) 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