diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp index 8e152183c8..c7c2bba7d7 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.cpp @@ -53,6 +53,7 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat // Motors _rotors.enablePropellerTorque(false); const bool rotors_added_successfully = _rotors.addActuators(configuration); + _forwards_motors_mask = _rotors.getForwardsMotors(); // Control Surfaces _first_control_surface_idx = configuration.num_actuators_matrix[0]; @@ -61,6 +62,13 @@ ActuatorEffectivenessFixedWing::getEffectivenessMatrix(Configuration &configurat return (rotors_added_successfully && surfaces_added_successfully); } +void ActuatorEffectivenessFixedWing::updateSetpoint(const matrix::Vector &control_sp, + int matrix_index, ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) +{ + stopMaskedMotorsWithZeroThrust(_forwards_motors_mask, actuator_sp); +} + void ActuatorEffectivenessFixedWing::allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) { diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp index 6195946866..5b4b7785b2 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessFixedWing.hpp @@ -51,6 +51,10 @@ public: void allocateAuxilaryControls(const float dt, int matrix_index, ActuatorVector &actuator_sp) override; + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, + ActuatorVector &actuator_sp, const matrix::Vector &actuator_min, + const matrix::Vector &actuator_max) override; + private: ActuatorEffectivenessRotors _rotors; ActuatorEffectivenessControlSurfaces _control_surfaces; @@ -59,4 +63,6 @@ private: uORB::Subscription _spoilers_setpoint_sub{ORB_ID(spoilers_setpoint)}; int _first_control_surface_idx{0}; ///< applies to matrix 1 + + uint32_t _forwards_motors_mask{}; }; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp index 1cf88c67b6..f123f47967 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp @@ -280,6 +280,21 @@ uint32_t ActuatorEffectivenessRotors::getUpwardsMotors() const return upwards_motors; } +uint32_t ActuatorEffectivenessRotors::getForwardsMotors() const +{ + uint32_t forward_motors = 0; + + for (int i = 0; i < _geometry.num_rotors; ++i) { + const Vector3f &axis = _geometry.rotors[i].axis; + + if (axis(0) > 0.5f && fabsf(axis(1)) < 0.1f && fabsf(axis(2)) < 0.1f) { + forward_motors |= 1u << i; + } + } + + return forward_motors; +} + bool ActuatorEffectivenessRotors::getEffectivenessMatrix(Configuration &configuration, EffectivenessUpdateReason external_update) diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index 8b6436cef1..5ca751e150 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -127,6 +127,7 @@ public: void enableThreeDimensionalThrust(bool enable) { _geometry.three_dimensional_thrust_disabled = !enable; } uint32_t getUpwardsMotors() const; + uint32_t getForwardsMotors() const; private: void updateParams() override;