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),