diff --git a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp index 24309f8adb..0ae567a54e 100644 --- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp +++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessTiltrotorVTOL.cpp @@ -60,8 +60,14 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config // MC motors configuration.selected_matrix = 0; _mc_rotors.enableYawControl(!_tilts.hasYawControl()); - _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, _last_tilt_control) + + // Update matrix with tilts in vertical position when update is triggered by a manual + // configuration (parameter) change (=force update). This is to make sure the normalization + // scales are tilt-invariant. Note: force update only possible when disarm. + const float tilt_control_applied = force ? -1.f : _last_tilt_control; + _nontilted_motors = _mc_rotors.updateAxisFromTilts(_tilts, tilt_control_applied) << configuration.num_actuators[(int)ActuatorType::MOTORS]; + _mc_rotors.getEffectivenessMatrix(configuration, true); // Control Surfaces @@ -75,7 +81,10 @@ ActuatorEffectivenessTiltrotorVTOL::getEffectivenessMatrix(Configuration &config _tilts.updateTorqueSign(_mc_rotors.geometry(), true /* disable pitch to avoid configuration errors */); _tilts.getEffectivenessMatrix(configuration, true); - _updated = false; + // If it was a forced update (thus coming from a config change), then make sure to update matrix in + // the next iteration again with the correct tilt (but without updating the normalization scale). + _updated = force; + return true; } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp index 4c9cd8161f..dfdf9ca7f7 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.cpp @@ -51,7 +51,8 @@ ControlAllocation::ControlAllocation() void ControlAllocation::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) { _effectiveness = effectiveness; ActuatorVector linearization_point_clipped = linearization_point; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp index d2c5e9489c..0d7895374e 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp @@ -104,7 +104,8 @@ public: * @param B Effectiveness matrix */ virtual void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators); + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale); /** * Get the allocated actuator vector diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index b93f0db46e..86cc198c01 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -44,10 +44,13 @@ void ControlAllocationPseudoInverse::setEffectivenessMatrix( const matrix::Matrix &effectiveness, - const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) { - ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators); + ControlAllocation::setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, num_actuators, + update_normalization_scale); _mix_update_needed = true; + _normalization_needs_update = update_normalization_scale; } void @@ -55,33 +58,31 @@ ControlAllocationPseudoInverse::updatePseudoInverse() { if (_mix_update_needed) { matrix::geninv(_effectiveness, _mix); + + if (_normalization_needs_update) { + updateControlAllocationMatrixScale(); + _normalization_needs_update = false; + } + normalizeControlAllocationMatrix(); _mix_update_needed = false; } } void -ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() +ControlAllocationPseudoInverse::updateControlAllocationMatrixScale() { + // Same scale on roll and pitch 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); - } - // 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; @@ -106,16 +107,31 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() _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; } +} +void +ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() +{ + 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(2) > FLT_EPSILON) { + _mix.col(2) /= _control_allocation_scale(2); + } + + if (_control_allocation_scale(3) > FLT_EPSILON) { + _mix.col(3) /= _control_allocation_scale(3); + _mix.col(4) /= _control_allocation_scale(4); + _mix.col(5) /= _control_allocation_scale(5); + } // Set all the small elements to 0 to avoid issues // in the control allocation algorithms diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index 6412b1cb11..27c367376b 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -55,7 +55,8 @@ public: void allocate() override; void setEffectivenessMatrix(const matrix::Matrix &effectiveness, - const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators) override; + const ActuatorVector &actuator_trim, const ActuatorVector &linearization_point, int num_actuators, + bool update_normalization_scale) override; protected: matrix::Matrix _mix; @@ -70,4 +71,6 @@ protected: private: void normalizeControlAllocationMatrix(); + void updateControlAllocationMatrixScale(); + bool _normalization_needs_update{false}; }; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp index 830d1b19a1..1f242befb7 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverseTest.cpp @@ -57,7 +57,7 @@ TEST(ControlAllocationTest, AllZeroCase) matrix::Vector linearization_point; matrix::Vector actuator_sp_expected; - method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16); + method.setEffectivenessMatrix(effectiveness, actuator_trim, linearization_point, 16, false); method.setControlSetpoint(control_sp); method.allocate(); method.clipActuatorSetpoint(); diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 18e67256de..9a570d6e52 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -496,7 +496,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(bool force) // Assign control effectiveness matrix int total_num_actuators = config.num_actuators_matrix[i]; _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i], - config.linearization_point[i], total_num_actuators); + config.linearization_point[i], total_num_actuators, force); } trims.timestamp = hrt_absolute_time();