diff --git a/src/lib/mixer_module/functions/FunctionMotors.hpp b/src/lib/mixer_module/functions/FunctionMotors.hpp index 65837cfa75..efba281884 100644 --- a/src/lib/mixer_module/functions/FunctionMotors.hpp +++ b/src/lib/mixer_module/functions/FunctionMotors.hpp @@ -76,7 +76,7 @@ public: static inline void updateValues(uint32_t reversible, float thrust_factor, float *values, int num_values) { - if (thrust_factor > 0.f && thrust_factor <= 1.f) { + if (thrust_factor > FLT_EPSILON && thrust_factor <= 1.f) { // thrust factor // rel_thrust = factor * x^2 + (1-factor) * x, const float a = thrust_factor; @@ -88,17 +88,15 @@ public: for (int i = 0; i < num_values; ++i) { - float control = values[i]; + const float control = values[i]; if (PX4_ISFINITE(control)) { - if (control > 0.f) { + if (control > FLT_EPSILON) { values[i] = -tmp1 + sqrtf(tmp2 + (control / a)); - } else if (control < -0.f) { + } else if (control < -FLT_EPSILON) { values[i] = tmp1 - sqrtf(tmp2 - (control / a)); - } else { - values[i] = 0.f; } } }