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

control_allocator: improve status output formatting (#26277)

Browse Source
This commit is contained in:
David Meng 2026-01-23 07:09:56 +08:00 committed by GitHub
parent 522c15284f
commit 11f4d5c4e7
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
1 changed files with 102 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

107
src/modules/control_allocator/ControlAllocator.cpp
View File

@ -826,13 +826,110 @@ int ControlAllocator::print_status()
PX4_INFO("Instance: %i", i);
}
PX4_INFO(" Effectiveness.T =");
effectiveness.T().print();
PX4_INFO(" Effectiveness =");
int num_configured = _control_allocation[i]->numConfiguredActuators();
// print column numbering
if (num_configured > 1) {
printf(" ");
for (int col = 0; col < num_configured; col++) {
printf("|%2u ", col);
}
printf("\n");
}
// Print effectiveness matrix with row labels
const char *row_labels[] = {"Mx", "My", "Mz", "Fx", "Fy", "Fz"};
for (int row = 0; row < 6; row++) {
printf("%2s|", row_labels[row]);
for (int col = 0; col < num_configured; col++) {
double d = static_cast<double>(effectiveness(row, col));
// avoid -0.0 for display
if (fabs(d - 0.0) < 1e-9) {
// print fixed width zero
printf(" 0 ");
} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
printf("% .1e ", d);
} else {
printf("% 6.5f ", d);
}
}
printf("\n");
}
PX4_INFO(" minimum =");
_control_allocation[i]->getActuatorMin().T().print();
// print column numbering
if (num_configured > 1) {
printf(" ");
for (int col = 0; col < num_configured; col++) {
printf("|%2u ", col);
}
printf("\n");
}
printf(" |");
for (int col = 0; col < num_configured; col++) {
double d = static_cast<double>(_control_allocation[i]->getActuatorMin()(col));
// avoid -0.0 for display
if (fabs(d - 0.0) < 1e-9) {
// print fixed width zero
printf(" 0 ");
} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
printf("% .1e ", d);
} else {
printf("% 6.5f ", d);
}
}
printf("\n");
PX4_INFO(" maximum =");
_control_allocation[i]->getActuatorMax().T().print();
PX4_INFO(" Configured actuators: %i", _control_allocation[i]->numConfiguredActuators());
// print column numbering
if (num_configured > 1) {
printf(" ");
for (int col = 0; col < num_configured; col++) {
printf("|%2u ", col);
}
printf("\n");
}
printf(" |");
for (int col = 0; col < num_configured; col++) {
double d = static_cast<double>(_control_allocation[i]->getActuatorMax()(col));
// avoid -0.0 for display
if (fabs(d - 0.0) < 1e-9) {
// print fixed width zero
printf(" 0 ");
} else if ((fabs(d) < 1e-4) || (fabs(d) >= 10.0)) {
printf("% .1e ", d);
} else {
printf("% 6.5f ", d);
}
}
printf("\n");
PX4_INFO(" Configured actuators: %i", num_configured);
}
if (_handled_motor_failure_bitmask) {