From 6aa2c0b6dcef6a2d61986c4851e844116b23bff1 Mon Sep 17 00:00:00 2001 From: Silvan Fuhrer Date: Tue, 28 May 2024 09:01:17 +0200 Subject: [PATCH] CA: change how normalization is done Instead of using the same normalization algorithm for multicopters and planes, this commit splits it up. The multicopter one is left unchanged in it's core (has special yaw handling e.g.), while the new (FW one) looks at all axes separately and scales the mixer matrix such that an input of 1 results in maximum actuator control setpoint. Signed-off-by: Silvan Fuhrer --- src/lib/matrix/matrix/Slice.hpp | 15 ++++ .../ControlAllocationPseudoInverse.cpp | 83 +++++++++++-------- .../ControlAllocationPseudoInverse.hpp | 1 + ...ontrolAllocationSequentialDesaturation.cpp | 57 +++++++------ 4 files changed, 96 insertions(+), 60 deletions(-) diff --git a/src/lib/matrix/matrix/Slice.hpp b/src/lib/matrix/matrix/Slice.hpp index 57f7cfcb70..e69380a51e 100644 --- a/src/lib/matrix/matrix/Slice.hpp +++ b/src/lib/matrix/matrix/Slice.hpp @@ -351,6 +351,21 @@ public: return min_val; } + // sum of absolute values + Type sum_abs() const + { + const SliceT &self = *this; + Type accum(0); + + for (size_t i = 0; i < P; i++) { + for (size_t j = 0; j < Q; j++) { + accum += std::abs(self(i, j)); + } + } + + return accum; + } + private: size_t _x0, _y0; MatrixT *_data; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp index 6f51b42471..38c7b4d3d4 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp @@ -107,34 +107,52 @@ ControlAllocationPseudoInverse::updateControlAllocationMatrixScale() // Scale yaw separately _control_allocation_scale(2) = _mix.col(2).max(); - } else { - _control_allocation_scale(0) = 1.f; - _control_allocation_scale(1) = 1.f; - _control_allocation_scale(2) = 1.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; - // 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 axis_idx = 2; axis_idx >= 0; --axis_idx) { + int num_non_zero_thrust = 0; + float norm_sum = 0.f; - for (int axis_idx = 2; axis_idx >= 0; --axis_idx) { - int num_non_zero_thrust = 0; - float norm_sum = 0.f; + for (int i = 0; i < _num_actuators; i++) { + float norm = fabsf(_mix(i, 3 + axis_idx)); + norm_sum += norm; - 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 (norm > FLT_EPSILON) { - ++num_non_zero_thrust; + if (num_non_zero_thrust > 0) { + _control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust; + + } else { + _control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z); } } - if (num_non_zero_thrust > 0) { - _control_allocation_scale(3 + axis_idx) = norm_sum / num_non_zero_thrust; + } else { + // Fixed-wing with control surfaces normalization + for (int i = 0; i < NUM_AXES; i++) { - } else { - _control_allocation_scale(3 + axis_idx) = _control_allocation_scale(THRUST_Z); + int num_non_zero_actuators = 0; + + for (int j = 0; j < _num_actuators; j++) { + + if (fabsf(_mix(j, i)) > 1e-3f) { + ++num_non_zero_actuators; + } + } + + const float axis_norm_scale = _mix.col(i).sum_abs(); + + if (num_non_zero_actuators > 0) { + _control_allocation_scale(i) = axis_norm_scale / num_non_zero_actuators; + + } else { + _control_allocation_scale(i) = 1.f; + } } } } @@ -142,27 +160,22 @@ ControlAllocationPseudoInverse::updateControlAllocationMatrixScale() void ControlAllocationPseudoInverse::normalizeControlAllocationMatrix() { - if (_control_allocation_scale(0) > FLT_EPSILON) { - _mix.col(0) /= _control_allocation_scale(0); - _mix.col(1) /= _control_allocation_scale(1); - } + // build the normalized matrix with the raw mixer matrix and the per-axis scales + _normalized_mix = _mix; - if (_control_allocation_scale(2) > FLT_EPSILON) { - _mix.col(2) /= _control_allocation_scale(2); - } - - if (_control_allocation_scale(3) > FLT_EPSILON) { - _mix.col(3) /= _control_allocation_scale(3); - _mix.col(4) /= _control_allocation_scale(4); - _mix.col(5) /= _control_allocation_scale(5); + // TODO check for tilting motors if normalization is correct + for (int i = 0; i < NUM_AXES; i++) { + if (_control_allocation_scale(i) > FLT_EPSILON) { + _normalized_mix.col(i) = _mix.col(i) / _control_allocation_scale(i); + } } // 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; + if (fabsf(_normalized_mix(i, j)) < 1e-3f) { + _normalized_mix(i, j) = 0.f; } } } @@ -177,5 +190,5 @@ ControlAllocationPseudoInverse::allocate() _prev_actuator_sp = _actuator_sp; // Allocate - _actuator_sp = _actuator_trim + _mix * (_control_sp - _control_trim); + _actuator_sp = _actuator_trim + _normalized_mix * (_control_sp - _control_trim); } diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp index 27c367376b..e7b5466ad9 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp @@ -60,6 +60,7 @@ public: protected: matrix::Matrix _mix; + matrix::Matrix _normalized_mix; bool _mix_update_needed{false}; diff --git a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp index 83be2a3b33..00070191c3 100644 --- a/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp +++ b/src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp @@ -128,12 +128,14 @@ ControlAllocationSequentialDesaturation::mixAirmodeRP() for (int i = 0; i < _num_actuators; i++) { _actuator_sp(i) = _actuator_trim(i) + - _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + - _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + - _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + - _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + - _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + _normalized_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _normalized_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _normalized_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim( + ControlAxis::THRUST_X)) + + _normalized_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim( + ControlAxis::THRUST_Y)) + + _normalized_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z); } desaturateActuators(_actuator_sp, thrust_z); @@ -153,14 +155,16 @@ ControlAllocationSequentialDesaturation::mixAirmodeRPY() for (int i = 0; i < _num_actuators; i++) { _actuator_sp(i) = _actuator_trim(i) + - _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + - _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + - _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + - _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + - _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + - _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); - yaw(i) = _mix(i, ControlAxis::YAW); + _normalized_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _normalized_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _normalized_mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)) + + _normalized_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim( + ControlAxis::THRUST_X)) + + _normalized_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim( + ControlAxis::THRUST_Y)) + + _normalized_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z); + yaw(i) = _normalized_mix(i, ControlAxis::YAW); } desaturateActuators(_actuator_sp, thrust_z); @@ -182,14 +186,16 @@ ControlAllocationSequentialDesaturation::mixAirmodeDisabled() for (int i = 0; i < _num_actuators; i++) { _actuator_sp(i) = _actuator_trim(i) + - _mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + - _mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + - _mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim(ControlAxis::THRUST_X)) + - _mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim(ControlAxis::THRUST_Y)) + - _mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); - roll(i) = _mix(i, ControlAxis::ROLL); - pitch(i) = _mix(i, ControlAxis::PITCH); + _normalized_mix(i, ControlAxis::ROLL) * (_control_sp(ControlAxis::ROLL) - _control_trim(ControlAxis::ROLL)) + + _normalized_mix(i, ControlAxis::PITCH) * (_control_sp(ControlAxis::PITCH) - _control_trim(ControlAxis::PITCH)) + + _normalized_mix(i, ControlAxis::THRUST_X) * (_control_sp(ControlAxis::THRUST_X) - _control_trim( + ControlAxis::THRUST_X)) + + _normalized_mix(i, ControlAxis::THRUST_Y) * (_control_sp(ControlAxis::THRUST_Y) - _control_trim( + ControlAxis::THRUST_Y)) + + _normalized_mix(i, ControlAxis::THRUST_Z) * (_control_sp(ControlAxis::THRUST_Z) - _control_trim(ControlAxis::THRUST_Z)); + thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z); + roll(i) = _normalized_mix(i, ControlAxis::ROLL); + pitch(i) = _normalized_mix(i, ControlAxis::PITCH); } // only reduce thrust @@ -211,9 +217,10 @@ ControlAllocationSequentialDesaturation::mixYaw() ActuatorVector thrust_z; for (int i = 0; i < _num_actuators; i++) { - _actuator_sp(i) += _mix(i, ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); - yaw(i) = _mix(i, ControlAxis::YAW); - thrust_z(i) = _mix(i, ControlAxis::THRUST_Z); + _actuator_sp(i) += _normalized_mix(i, + ControlAxis::YAW) * (_control_sp(ControlAxis::YAW) - _control_trim(ControlAxis::YAW)); + yaw(i) = _normalized_mix(i, ControlAxis::YAW); + thrust_z(i) = _normalized_mix(i, ControlAxis::THRUST_Z); } // Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),