Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 03:49:12 +08:00
Code Issues Actions 6 Packages Projects Releases Wiki Activity

ControlAllocator: Set all the elements of a row to 0 if that row has weak authority

Browse Source 
Weak authority on a axis is currently defined as: none of the actuators have an
effectivness on this particular axis larger than 0.05.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-02-15 13:29:53 +01:00
committed by Beat Küng
parent 97a280d41d
commit c51dc3b4b7
1 changed files with 14 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+14
View File
@@ -494,6 +494,20 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
_control_allocation[i]->setActuatorMax(maximum[i]);
_control_allocation[i]->setSlewRateLimit(slew_rate[i]);
// Set all the elements of a row to 0 if that row has weak authority.
// That ensures that the algorithm doesn't try to control axes with only marginal control authority,
// which in turn would degrade the control of the main axes that actually should and can be controlled.
ActuatorEffectiveness::EffectivenessMatrix &matrix = config.effectiveness_matrices[i];
for (int n = 0; n < NUM_AXES; n++) {
if (matrix.row(i).max() < 0.05f) {
for (int m = 0; m < _num_actuators[i]; m++) {
matrix(n, m) = 0.f;
}
}
}
// Assign control effectiveness matrix
int total_num_actuators = config.num_actuators_matrix[i];
_control_allocation[i]->setEffectivenessMatrix(config.effectiveness_matrices[i], config.trim[i],