From 960003a86ada98bdc0ad19e88c9dbc33001a13e3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Mon, 21 Feb 2022 09:44:32 +0100 Subject: [PATCH] control_allocator: fix weak authority matrix check & update - use max(fabsf(val)) instead of max(val) - use correct actuator count --- src/modules/control_allocator/ControlAllocator.cpp | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp index 8825a91ff8..ab25fd5381 100644 --- a/src/modules/control_allocator/ControlAllocator.cpp +++ b/src/modules/control_allocator/ControlAllocator.cpp @@ -501,11 +501,17 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso 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; + bool all_entries_small = true; + + for (int m = 0; m < config.num_actuators_matrix[i]; m++) { + if (fabsf(matrix(n, m)) > 0.05f) { + all_entries_small = false; } } + + if (all_entries_small) { + matrix.row(n) = 0.f; + } } // Assign control effectiveness matrix