fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-11-11 04:31:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

CA: change how normalization is done

Browse Source 
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 <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-06-10 11:15:27 +02:00
parent f75a0bec33
commit 6aa2c0b6dc
4 changed files with 96 additions and 60 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/matrix/matrix/Slice.hpp
+15
View File
@@ -351,6 +351,21 @@ public:
return min_val;
}
// sum of absolute values
Type sum_abs() const
{
const SliceT<MatrixT, Type, P, Q, M, N> &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;
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
+48 -35
View File
@@ -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);
}
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp
+1
View File
@@ -60,6 +60,7 @@ public:
protected:
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _normalized_mix;
bool _mix_update_needed{false};
src/modules/control_allocator/ControlAllocation/ControlAllocationSequentialDesaturation.cpp
+32 -25
View File
@@ -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),