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

mixer: FunctionMotors: leave NAN control values at NAN with non-zero THR_MDL_FAC

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer
2024-12-13 17:56:46 +01:00
committed by Matthias Grob
parent 3941b19968
commit d7904b5f2c
1 changed files with 9 additions and 6 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/mixer_module/functions/FunctionMotors.hpp
+9 -6
View File
@@ -87,16 +87,19 @@ public:
const float tmp2 = b * b / (4.f * a * a); const float tmp2 = b * b / (4.f * a * a);
for (int i = 0; i < num_values; ++i) { for (int i = 0; i < num_values; ++i) {
float control = values[i]; float control = values[i];
if (control > 0.f) { if (PX4_ISFINITE(control)) {
values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); if (control > 0.f) {
values[i] = -tmp1 + sqrtf(tmp2 + (control / a));
} else if (control < -0.f) { } else if (control < -0.f) {
values[i] = tmp1 - sqrtf(tmp2 - (control / a)); values[i] = tmp1 - sqrtf(tmp2 - (control / a));
} else { } else {
values[i] = 0.f; values[i] = 0.f;
}
} }
} }
} }