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

ControlAllocation: fix calculation of roll/pitch normalization scale

Browse Source 
Take into account the actual number of roll and pitch acutators,
instead of assuming that all actuators have a roll/pitch component.

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2022-01-10 16:47:59 +01:00
committed by Beat Küng
parent 0950bb81ab
commit cbb8c90245
1 changed files with 27 additions and 3 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ControlAllocation/ControlAllocationPseudoInverse.cpp
+27 -3
View File
@@ -74,10 +74,34 @@ ControlAllocationPseudoInverse::updateControlAllocationMatrixScale()
{
// Same scale on roll and pitch
if (_normalize_rpy) {
const float roll_norm_sq = _mix.col(0).norm_squared();
const float pitch_norm_sq = _mix.col(1).norm_squared();
_control_allocation_scale(0) = sqrtf(fmaxf(roll_norm_sq, pitch_norm_sq) / (_num_actuators / 2.f));
int num_non_zero_roll_torque = 0;
int num_non_zero_pitch_torque = 0;
for (int i = 0; i < _num_actuators; i++) {
if (fabsf(_mix(i, 0)) > 1e-3f) {
++num_non_zero_roll_torque;
}
if (fabsf(_mix(i, 1)) > 1e-3f) {
++num_non_zero_pitch_torque;
}
}
float roll_norm_scale = 1.f;
if (num_non_zero_roll_torque > 0) {
roll_norm_scale = sqrtf(_mix.col(0).norm_squared() / (num_non_zero_roll_torque / 2.f));
}
float pitch_norm_scale = 1.f;
if (num_non_zero_pitch_torque > 0) {
pitch_norm_scale = sqrtf(_mix.col(1).norm_squared() / (num_non_zero_pitch_torque / 2.f));
}
_control_allocation_scale(0) = fmaxf(roll_norm_scale, pitch_norm_scale);
_control_allocation_scale(1) = _control_allocation_scale(0);
// Scale yaw separately