From cbb8c902450417473cdb9f19008a552e934eb7b5 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Mon, 10 Jan 2022 16:47:59 +0100 Subject: [PATCH] ControlAllocation: fix calculation of roll/pitch normalization scale Take into account the actual number of roll and pitch acutators, instead of assuming that all actuators have a roll/pitch component. Signed-off-by: Silvan Fuhrer --- .../ControlAllocationPseudoInverse.cpp | 30 +++++++++++++++++-- 1 file changed, 27 insertions(+), 3 deletions(-) diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 86cc198c01..81b4741bc9 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -74,10 +74,34 @@ ControlAllocationPseudoInverse::updateControlAllocationMatrixScale() { // Same scale on roll and pitch if (_normalize_rpy) { - 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)); + int num_non_zero_roll_torque = 0; + int num_non_zero_pitch_torque = 0; + + for (int i = 0; i < _num_actuators; i++) { + + if (fabsf(_mix(i, 0)) > 1e-3f) { + ++num_non_zero_roll_torque; + } + + if (fabsf(_mix(i, 1)) > 1e-3f) { + ++num_non_zero_pitch_torque; + } + } + + float roll_norm_scale = 1.f; + + if (num_non_zero_roll_torque > 0) { + roll_norm_scale = sqrtf(_mix.col(0).norm_squared() / (num_non_zero_roll_torque / 2.f)); + } + + float pitch_norm_scale = 1.f; + + if (num_non_zero_pitch_torque > 0) { + pitch_norm_scale = sqrtf(_mix.col(1).norm_squared() / (num_non_zero_pitch_torque / 2.f)); + } + + _control_allocation_scale(0) = fmaxf(roll_norm_scale, pitch_norm_scale); _control_allocation_scale(1) = _control_allocation_scale(0); // Scale yaw separately