From 866f9fa95b7b0f2bd2b8ff732a3769f3e4b61928 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 9 May 2022 15:29:38 +0200 Subject: [PATCH] Control Allocation: add option to disable yaw by differential thrust This replaces the propeller_torque_disabled flag to disable yaw by differential thrust for tiltrotor and tailsitter VTOLs, as propeller_torque_disabled is not enough to set effectiveness of an acutator in the yaw axis to 0 in case it's tilted. Signed-off-by: Silvan Fuhrer --- .../ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp | 2 +- .../ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp | 5 +++++ .../ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp | 3 +++ .../ActuatorEffectivenessTailsitterVTOL.cpp | 3 ++- .../ActuatorEffectivenessTiltrotorVTOL.cpp | 2 +- 5 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp index da1ebdb9b6..4ac953b53b 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.cpp @@ -51,7 +51,7 @@ ActuatorEffectivenessMCTilt::getEffectivenessMatrix(Configuration &configuration } // MC motors - _mc_rotors.enablePropellerTorque(!_tilts.hasYawControl()); + _mc_rotors.enableYawByDifferentialThrust(!_tilts.hasYawControl()); const bool rotors_added_successfully = _mc_rotors.addActuators(configuration); // Tilts diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index a35a448fe5..65b987aba4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -213,6 +213,11 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry effectiveness(j + 3, i + actuator_start_index) = thrust(j); } + if (geometry.yaw_by_differential_thrust_disabled) { + // set yaw effectiveness to 0 if yaw is controlled by other means (e.g. tilts) + effectiveness(2, i + actuator_start_index) = 0.f; + } + 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 diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 7e118428c1..8b6436cef1 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -74,6 +74,7 @@ public: RotorGeometry rotors[NUM_ROTORS_MAX]; int num_rotors{0}; bool propeller_torque_disabled{false}; + bool yaw_by_differential_thrust_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 }; @@ -119,6 +120,8 @@ public: void enablePropellerTorque(bool enable) { _geometry.propeller_torque_disabled = !enable; } + void enableYawByDifferentialThrust(bool enable) { _geometry.yaw_by_differential_thrust_disabled = !enable; } + void enablePropellerTorqueNonUpwards(bool enable) { _geometry.propeller_torque_disabled_non_upwards = !enable; } void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp index 9710db8465..2683c84e77 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.cpp @@ -56,7 +56,8 @@ ActuatorEffectivenessTailsitterVTOL::getEffectivenessMatrix(Configuration &confi // MC motors configuration.selected_matrix = 0; - _mc_rotors.enablePropellerTorque(_mc_rotors.geometry().num_rotors > 3); // enable MC yaw control if more than 3 rotors + // enable MC yaw control if more than 3 rotors + _mc_rotors.enableYawByDifferentialThrust(_mc_rotors.geometry().num_rotors > 3); const bool mc_rotors_added_successfully = _mc_rotors.addActuators(configuration); // Control Surfaces diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 52bd0b48c8..675548cab6 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -60,7 +60,7 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // MC motors configuration.selected_matrix = 0; - _mc_rotors.enablePropellerTorque(!_tilts.hasYawControl()); + _mc_rotors.enableYawByDifferentialThrust(!_tilts.hasYawControl()); _mc_rotors.enableThreeDimensionalThrust(false); // Update matrix with tilts in vertical position when update is triggered by a manual