fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-19 23:59:05 +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
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
src/modules/control_allocator/ControlAllocation/ControlAllocation.hpp
View File

@ -220,6 +220,9 @@ public:
void setNormalizeAsPlanarMC(bool normalize_as_mc) { _normalize_matrix_as_planar_mc = normalize_as_mc; }
void setAirmode(const int airmode) {_airmode = airmode; }
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> getMixMatrix() {return _mix; }
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> getNormalizedMixMatrix() {return _normalized_mix; }
protected:
friend class ControlAllocator; // for _actuator_sp
@ -236,4 +239,6 @@ protected:
int _num_actuators{0};
bool _normalize_matrix_as_planar_mc{false}; ///< if true, normalize roll, pitch and yaw columns optimized for planar MC
int _airmode{0}; ///< 0: disabled, 1: RP airmode, 2: RPY airmode
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _normalized_mix;
};

2
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.hpp
View File

@ -59,8 +59,6 @@ public:
bool update_normalization_scale) override;
protected:
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _mix;
matrix::Matrix<float, NUM_ACTUATORS, NUM_AXES> _normalized_mix;
bool _mix_update_needed{false};

24
src/modules/control_allocator/ControlAllocator.cpp
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 =");