diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 35b561c4e6..4134dd0c51 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -209,10 +209,10 @@ MultirotorMixer::mix(float *outputs, unsigned space) // which may result in motor saturation after thrust boost is applied // TODO: revise the saturation/boosting strategy - if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) { + if (min_out < 0.0f && max_out < 1.0f && max_out - min_out <= 1.0f) { boost = -min_out; - } else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) { + } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out <= 1.0f) { float max_thrust_diff = thrust - thrust_decrease_factor * thrust; if (max_thrust_diff >= max_out - 1.0f) { @@ -223,12 +223,12 @@ MultirotorMixer::mix(float *outputs, unsigned space) roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); } - } else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) { + } else if (min_out < 0.0f && max_out < 1.0f && max_out - min_out > 1.0f) { float max_thrust_diff = thrust * thrust_increase_factor - thrust; boost = math::constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff); roll_pitch_scale = (thrust + boost) / (thrust - min_out); - } else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) { + } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out > 1.0f) { float max_thrust_diff = thrust - thrust_decrease_factor * thrust; boost = math::constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f); roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);