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

control_allocator: fix weak authority matrix check & update

Browse Source 
- use max(fabsf(val)) instead of max(val)
- use correct actuator count
This commit is contained in:
Beat Küng
2022-02-21 09:44:32 +01:00
committed by Daniel Agar
parent 591b7b6934
commit 960003a86a
1 changed files with 9 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocator.cpp
+9 -3
View File
@@ -501,11 +501,17 @@ ControlAllocator::update_effectiveness_matrix_if_needed(EffectivenessUpdateReaso
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;
bool all_entries_small = true;
for (int m = 0; m < config.num_actuators_matrix[i]; m++) {
if (fabsf(matrix(n, m)) > 0.05f) {
all_entries_small = false;
}
}
if (all_entries_small) {
matrix.row(n) = 0.f;
}
}
// Assign control effectiveness matrix