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

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

bash 复制代码

// Handle failed actuators
		if (_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

那么如果六旋翼的原始矩阵是

bash 复制代码

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 ]

那么经过上面的一段代码之后

最新的矩阵就是这个

bash 复制代码

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 ]

过程是这样的

bash 复制代码

电机失效处理逻辑：  
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 (非零，条件成立)

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

bash 复制代码

--- 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;
 }

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

bash 复制代码

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 actuators
                if (_handled_motor_failure_bitmask) {
                        actuator_idx = 0;
@@ -594,6 +596,7 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
                trims.timestamp = hrt_absolute_time();
                _actuator_servos_trim_pub.publish(trims);
        }