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

CA: extend status print

Browse Source 
Print not only effectiveness matrix, but also raw and normalized
mixing matrix (inverted effectiveness).

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-06-06 11:34:26 +02:00
parent 1fcd5b466f
commit 9bdc898ceb
3 changed files with 27 additions and 4 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+22 -2
View File
@@ -931,8 +931,28 @@ int ControlAllocator::print_status()
PX4_INFO("Instance: %i", i);
}
PX4_INFO(" Effectiveness.T =");
effectiveness.T().print();
// const size_t num_actuators = _control_allocation[i]->numConfiguredActuators();
const size_t num_actuators = 6;
matrix::Matrix<float, num_actuators, NUM_AXES> effectiveness_sliced_transposed(
effectiveness.T().slice<num_actuators, NUM_AXES>(0, 0));
const matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> mix_raw = _control_allocation[i]->getMixMatrix();
const matrix::Matrix<float, num_actuators, NUM_AXES> mix_raw_sliced(mix_raw.slice<num_actuators, NUM_AXES>(0, 0));
const matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> mix_normalized = _control_allocation[i]->getNormalizedMixMatrix();
const matrix::Matrix<float, num_actuators, NUM_AXES> mix_normalized_sliced(
mix_normalized.slice<num_actuators, NUM_AXES>(0, 0));
PX4_INFO(" Effectiveness.T (sliced) =");
effectiveness_sliced_transposed.print();
PX4_INFO(" Mixer matrix raw (sliced) =");
mix_raw_sliced.print();
PX4_INFO(" Mixer matrix normalized (sliced) =");
mix_normalized_sliced.print();
PX4_INFO(" minimum =");
_control_allocation[i]->getActuatorMin().T().print();
PX4_INFO(" maximum =");