From c51dc3b4b7dd65febb5918ba822ca32ed1d39173 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 15 Feb 2022 13:29:53 +0100 Subject: [PATCH] ControlAllocator: Set all the elements of a row to 0 if that row has weak authority Weak authority on a axis is currently defined as: none of the actuators have an effectivness on this particular axis larger than 0.05. Signed-off-by: Silvan Fuhrer --- src/modules/control_allocator/ControlAllocator.cpp | 14 ++++++++++++++ 1 file changed, 14 insertions(+) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index ab3d339f87..8825a91ff8 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -494,6 +494,20 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso _control_allocation[i]->setActuatorMax(maximum[i]); _control_allocation[i]->setSlewRateLimit(slew_rate[i]); + // Set all the elements of a row to 0 if that row has weak authority. + // That ensures that the algorithm doesn't try to control axes with only marginal control authority, + // which in turn would degrade the control of the main axes that actually should and can be controlled. + + ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[i]; + + for (int n = 0; n < NUM_AXES; n++) { + if (matrix.row(i).max() < 0.05f) { + for (int m = 0; m < _num_actuators[i]; m++) { + matrix(n, m) = 0.f; + } + } + } + // Assign control effectiveness matrix int total_num_actuators = config.num_actuators_matrix[i]; _control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],