From 0728034474032a137ed9d114d315fbf005a559c2 Mon Sep 17 00:00:00 2001 From: bresch Date: Tue, 5 Sep 2017 11:26:06 +0200 Subject: [PATCH] Multicopter mixer - Simplify and correct mistakes of roll-pitch motor saturation handling --- src/lib/mixer/mixer_multirotor.cpp | 31 +++++++++++++++--------------- 1 file changed, 16 insertions(+), 15 deletions(-) diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp index 1322f563f18f..68d471156370 100644 --- a/src/lib/mixer/mixer_multirotor.cpp +++ b/src/lib/mixer/mixer_multirotor.cpp @@ -199,31 +199,32 @@ MultirotorMixer::mix(float *outputs, unsigned space) float boost = 0.0f; // value added to demanded thrust (can also be negative) float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch + float delta_out_max = max_out - min_out; // distance between the two extrema + // If the difference between the to extrema is smaller than 1.0, the boost can safely unsaturate a motor if needed + // without saturating another one. + // Otherwise, a scaler is computed to make the distance between the two extrema exacly 1.0 and the boost + // value is computed to maximize the roll-pitch control. + // // Note: thrust boost is computed assuming thrust_gain==1 for all motors. // On asymmetric platforms, some motors have thrust_gain<1, // which may result in motor saturation after thrust boost is applied // TODO: revise the saturation/boosting strategy + if (delta_out_max <= 1.0f) { + if (min_out < 0.0f) { + boost = -min_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) { + boost = -(max_out - 1.0f); - } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out <= 1.0f) { - boost = -(max_out - 1.0f); - - } else if (min_out < 0.0f && max_out < 1.0f && max_out - min_out > 1.0f) { - boost = -min_out - (1.0f - max_out) / 2.0f; - roll_pitch_scale = (thrust + boost) / (thrust - min_out); - - } else if (max_out > 1.0f && min_out > 0.0f && max_out - min_out > 1.0f) { - boost = -(max_out - min_out - 1.0f) / 2.0f; - roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); + } - } else if (min_out < 0.0f && max_out > 1.0f) { - boost = -(max_out - 1.0f + min_out) / 2.0f; - roll_pitch_scale = (thrust + boost) / (thrust - min_out); + } else { + roll_pitch_scale = 1 / (max_out - min_out); + boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust); } + // capture saturation if (min_out < 0.0f) { _saturation_status.flags.motor_neg = true;