关于px4 1.15.0电机控制有效矩阵的更新

下面这段代码是PX4飞控根据_handled_motor_failure_bitmask的值， 更新无人机的有效性矩阵的代码。

// Handle failed actuatorsif (_handled_motor_failure_bitmask) {actuator_idx = 0;memset(&actuator_idx_matrix, 0, sizeof(actuator_idx_matrix));for (int motors_idx = 0; motors_idx < _num_actuators[0] && motors_idx < actuator_motors_s::NUM_CONTROLS; motors_idx++) {int selected_matrix = _control_allocation_selection_indexes[actuator_idx];if (_handled_motor_failure_bitmask & (1 << motors_idx)) {ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[selected_matrix];for (int i = 0; i < NUM_AXES; i++) {matrix(i, actuator_idx_matrix[selected_matrix]) = 0.0f;}}++actuator_idx_matrix[selected_matrix];++actuator_idx;}}

假设我们停的是5号电机。 那么_handled_motor_failure_bitmask = 0x10
那么如果六旋翼的原始矩阵是

Row 0: [-6.500  6.500  3.250 -3.250 -3.250  3.250 ]
Row 1: [ 0.000  0.000  5.629 -5.629  5.629 -5.629 ]
Row 2: [-0.325  0.325 -0.325  0.325  0.325 -0.325 ]
Row 3: [ 0.000  0.000  0.000  0.000  0.000  0.000 ]
Row 4: [ 0.000  0.000  0.000  0.000  0.000  0.000 ]
Row 5: [-6.500 -6.500 -6.500 -6.500 -6.500 -6.500 ]

那么经过上面的一段代码之后
最新的矩阵就是这个

Row 0: [-6.500  6.500  3.250 -3.250  0.000  3.250 ]
Row 1: [ 0.000  0.000  5.629 -5.629  0.000 -5.629 ]
Row 2: [-0.325  0.325 -0.325  0.325  0.000 -0.325 ]
Row 3: [ 0.000  0.000  0.000  0.000  0.000  0.000 ]
Row 4: [ 0.000  0.000  0.000  0.000  0.000  0.000 ]
Row 5: [-6.500 -6.500 -6.500 -6.500  0.000 -6.500 ]

过程是这样的

电机失效处理逻辑：  
1. 电机编号对应： • motors_idx = 0 → 第1列 (电机0) • motors_idx = 1 → 第2列 (电机1) • motors_idx = 2 → 第3列 (电机2) • motors_idx = 3 → 第4列 (电机3)  
• motors_idx = 4 → 第5列 (电机4) ← 失效 • motors_idx = 5 → 第6列 (电机5)  2. 失效处理：  3. • 当 motors_idx = 4 时，_handled_motor_failure_bitmask & (1 <<4) = 0x10 & 0x10 = 0x10 (非零，条件成立)

有效矩阵怎么在飞控代码中打印出来呢？

--- a/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
+++ b/src/modules/control_allocator/ActuatorEffectiveness/ActuatorEffectivenessRotors.cpp
@@ -221,6 +221,22 @@ ActuatorEffectivenessRotors::computeEffectivenessMatrix(const Geometry &geometry}}+
+
+       // 打印根据机体结构计算的初始有效矩阵
+       PX4_INFO("=== Initial Effectiveness Matrix (from Geometry) ===");
+       PX4_INFO("Actuator start index: %d, Num actuators: %d", actuator_start_index, num_actuators);
+       for (int row = 0; row < 6; row++) {
+               char row_str[200] = "";
+               for (int col = 0; col < num_actuators; col++) {
+                       char temp[20];
+                       snprintf(temp, sizeof(temp), "%.3f ", (double)effectiveness(row, col + actuator_start_index));
+                       strcat(row_str, temp);
+               }
+               PX4_INFO("Row %d: [%s]", row, row_str);
+       }
+
+return num_actuators;}

更新后的矩阵，应该在下面的函数中，打印的， 但是打印代码始终不对，后面再添加。

diff --git a/src/modules/control_allocator/ControlAllocator.cpp b/src/modules/control_allocator/ControlAllocator.cpp
index 6ac6f318..1c9a5739 100644
--- a/src/modules/control_allocator/ControlAllocator.cpp
+++ b/src/modules/control_allocator/ControlAllocator.cpp
@@ -539,6 +539,8 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso}}+               printf("cuigaosheng  _handled_motor_failure_bitmask: 0x%x", _handled_motor_failure_bitmask);
+// Handle failed actuatorsif (_handled_motor_failure_bitmask) {actuator_idx = 0;
@@ -594,6 +596,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReasotrims.timestamp = hrt_absolute_time();_actuator_servos_trim_pub.publish(trims);}