Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-25 08:36:08 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

control_allocator: compute thrust scaling individually per axis

Browse Source 
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.
This commit is contained in:
Beat Küng
2022-02-16 11:09:02 +01:00
parent 76d8d8cae6
commit ddad4c31c9
1 changed files with 18 additions and 18 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
+18 -18
View File
@@ -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);
}
}
}