From ba1b7f3a07fecc4f8707e53ab05278f0721e8972 Mon Sep 17 00:00:00 2001 From: bresch Date: Wed, 6 Oct 2021 15:21:49 +0200 Subject: [PATCH] CA pseudo inverse: set all small elements in CA matrix to zero This avoids problems in the sequencial desaturation method where vectors of the CA matrix are used to desaturate the motors. --- .../ControlAllocationPseudoInverse.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 9d58f8f12d..4ccf51167d 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -99,6 +99,16 @@ ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() if ((-z_sum > FLT_EPSILON) && (_num_actuators > 0)) { _mix.col(5) /= -z_sum / _num_actuators; } + + // Set all the small elements to 0 to avoid issues + // in the control allocation algorithms + for (int i = 0; i < _num_actuators; i++) { + for (int j = 0; j < NUM_AXES; j++) { + if (fabsf(_mix(i, j)) < 1e-3f) { + _mix(i, j) = 0.f; + } + } + } } void