diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp index 272eabf7e7..a80ce0113d 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectiveness.hpp @@ -132,6 +132,16 @@ public: } } + /** + * Query if the roll, pitch and yaw columns of the mixing matrix should be normalized + */ + virtual void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const + { + for (int i = 0; i < MAX_NUM_MATRICES; ++i) { + normalize[i] = false; + } + } + /** * Get the control effectiveness matrix if updated * diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp index 191ad4b396..4bdae41efe 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessMCTilt.hpp @@ -50,6 +50,11 @@ public: allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; } + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + } + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp) override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp index a60efb0e76..0625d74628 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.hpp @@ -85,6 +85,11 @@ public: allocation_method_out[0] = AllocationMethod::SEQUENTIAL_DESATURATION; } + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + } + static int computeEffectivenessMatrix(const Geometry &geometry, EffectivenessMatrix &effectiveness, int actuator_start_index = 0); diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp index 88e6c70a18..50b84a9083 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessStandardVTOL.hpp @@ -66,6 +66,12 @@ public: allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE; } + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + normalize[1] = false; + } + void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, ActuatorVector &actuator_sp) override; diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp index 49ba6d8974..58c01fcfd4 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTailsitterVTOL.hpp @@ -63,6 +63,12 @@ public: allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE; } + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + normalize[1] = false; + } + void setFlightPhase(const FlightPhase &flight_phase) override; const char *name() const override { return "VTOL Tailsitter"; } diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp index ab808b5cc3..705c596acb 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.hpp @@ -66,6 +66,12 @@ public: allocation_method_out[1] = AllocationMethod::PSEUDO_INVERSE; } + void getNormalizeRPY(bool normalize[MAX_NUM_MATRICES]) const override + { + normalize[0] = true; + normalize[1] = false; + } + void setFlightPhase(const FlightPhase &flight_phase) override; void updateSetpoint(const matrix::Vector &control_sp, int matrix_index, diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index 7fc7d7b8f2..d2c5e9489c 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -216,6 +216,8 @@ public: int numConfiguredActuators() const { return _num_actuators; } + void setNormalizeRPY(bool normalize_rpy) { _normalize_rpy = normalize_rpy; } + protected: friend class ControlAllocator; // for _actuator_sp @@ -230,4 +232,5 @@ protected: matrix::Vector _control_sp; ///< Control setpoint matrix::Vector _control_trim; ///< Control at trim actuator values int _num_actuators{0}; + bool _normalize_rpy{false}; ///< if true, normalize roll, pitch and yaw columns }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 9bf46eeee8..b93f0db46e 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -63,46 +63,60 @@ ControlAllocationPseudoInverse::updatePseudoInverse() void ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() { - // Same scale on roll and pitch - const float roll_norm_sq = _mix.col(0).norm_squared(); - const float pitch_norm_sq = _mix.col(1).norm_squared(); - _control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f)); - _control_allocation_scale(1) = _control_allocation_scale(0); + if (_normalize_rpy) { + // Same scale on roll and pitch + const float roll_norm_sq = _mix.col(0).norm_squared(); + const float pitch_norm_sq = _mix.col(1).norm_squared(); + _control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f)); + _control_allocation_scale(1) = _control_allocation_scale(0); - if (_control_allocation_scale(0) > FLT_EPSILON) { - _mix.col(0) /= _control_allocation_scale(0); - _mix.col(1) /= _control_allocation_scale(1); + if (_control_allocation_scale(0) > FLT_EPSILON) { + _mix.col(0) /= _control_allocation_scale(0); + _mix.col(1) /= _control_allocation_scale(1); + } + + // Scale yaw separately + _control_allocation_scale(2) = _mix.col(2).max(); + + if (_control_allocation_scale(2) > FLT_EPSILON) { + _mix.col(2) /= _control_allocation_scale(2); + } + + } else { + _control_allocation_scale(0) = 1.f; + _control_allocation_scale(1) = 1.f; + _control_allocation_scale(2) = 1.f; } - // Scale yaw separately - _control_allocation_scale(2) = _mix.col(2).max(); - - if (_control_allocation_scale(2) > FLT_EPSILON) { - _mix.col(2) /= _control_allocation_scale(2); - } - - // Same scale on X and Y - _control_allocation_scale(3) = fmaxf(_mix.col(3).max(), _mix.col(4).max()); - _control_allocation_scale(4) = _control_allocation_scale(3); - - if (_control_allocation_scale(3) > FLT_EPSILON) { - _mix.col(3) /= _control_allocation_scale(3); - _mix.col(4) /= _control_allocation_scale(4); - } - - // Scale Z thrust separately - float z_sum = 0.f; - auto z_col = _mix.col(5); + // Scale thrust by the sum of the norm of the thrust vectors (which is invariant to rotation) + int num_non_zero_thrust = 0; + float norm_sum = 0.f; for (int i = 0; i < _num_actuators; i++) { - z_sum += z_col(i, 0); + float norm = _mix.slice<1, 3>(i, 3).norm(); + norm_sum += norm; + + if (norm > FLT_EPSILON) { + ++num_non_zero_thrust; + } } - if ((-z_sum > FLT_EPSILON) && (_num_actuators > 0)) { - _control_allocation_scale(5) = -z_sum / _num_actuators; + if (num_non_zero_thrust > 0) { + norm_sum /= num_non_zero_thrust; + _control_allocation_scale(3) = norm_sum; + _control_allocation_scale(4) = norm_sum; + _control_allocation_scale(5) = norm_sum; + _mix.col(3) /= _control_allocation_scale(3); + _mix.col(4) /= _control_allocation_scale(4); _mix.col(5) /= _control_allocation_scale(5); + + } else { + _control_allocation_scale(3) = 1.f; + _control_allocation_scale(4) = 1.f; + _control_allocation_scale(5) = 1.f; } + // Set all the small elements to 0 to avoid issues // in the control allocation algorithms for (int i = 0; i < _num_actuators; i++) { diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index ceba598bce..8a403d6f14 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -157,6 +157,9 @@ ControlAllocator::update_allocation_method(bool force) AllocationMethod desired_methods[ActuatorEffectiveness::MAX_NUM_MATRICES]; _actuator_effectiveness->getDesiredAllocationMethod(desired_methods); + bool normalize_rpy[ActuatorEffectiveness::MAX_NUM_MATRICES]; + _actuator_effectiveness->getNormalizeRPY(normalize_rpy); + for (int i = 0; i < _num_control_allocation; ++i) { AllocationMethod method = configured_method; @@ -183,6 +186,7 @@ ControlAllocator::update_allocation_method(bool force) _num_control_allocation = 0; } else { + _control_allocation[i]->setNormalizeRPY(normalize_rpy[i]); _control_allocation[i]->setActuatorSetpoint(actuator_sp[i]); } }