From ddad4c31c901cc0119bf223fa140d278a9571a18 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Beat=20K=C3=BCng?= Date: Wed, 16 Feb 2022 11:09:02 +0100 Subject: [PATCH] control_allocator: compute thrust scaling individually per axis Before, adding the pusher to the same matrix as the upwards motors affected the scaling for the upwards motors, resulting in values not equal to -1 anymore. --- .../ControlAllocationPseudoInverse.cpp | 36 +++++++++---------- 1 file changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 81b4741bc9..65fd35d98e 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -113,29 +113,29 @@ ControlAllocationPseudoInverse::updateControlAllocationMatrixScale() _control_allocation_scale(2) = 1.f; } - // 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; + // Scale thrust by the sum of the individual thrust axes, and use the scaling for the Z axis if there's no actuators + // (for tilted actuators) + _control_allocation_scale(THRUST_Z) = 1.f; - for (int i = 0; i < _num_actuators; i++) { - float norm = _mix.slice<1, 3>(i, 3).norm(); - norm_sum += norm; + for (int axis_idx = 2; axis_idx >= 0; --axis_idx) { + int num_non_zero_thrust = 0; + float norm_sum = 0.f; - if (norm > FLT_EPSILON) { - ++num_non_zero_thrust; + for (int i = 0; i < _num_actuators; i++) { + float norm = fabsf(_mix(i, 3 + axis_idx)); + norm_sum += norm; + + if (norm > FLT_EPSILON) { + ++num_non_zero_thrust; + } } - } - 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; + if (num_non_zero_thrust > 0) { + _control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust; - } else { - _control_allocation_scale(3) = 1.f; - _control_allocation_scale(4) = 1.f; - _control_allocation_scale(5) = 1.f; + } else { + _control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z); + } } }