diff --git a/src/lib/mixer/mixer.h b/src/lib/mixer/mixer.h
index 336c217161..fcf065a11a 100644
--- a/src/lib/mixer/mixer.h
+++ b/src/lib/mixer/mixer.h
@@ -693,6 +693,78 @@ public:
 	};
 
 private:
+	/**
+	 * Computes the gain k by which delta_outputs has to be multiplied
+	 * in order to unsaturate the output that has the greatest saturation.
+	 * @see also minimize_saturation().
+	 *
+	 * @return desaturation gain
+	 */
+	float compute_desaturation_gain(const float *delta_outputs, const float *outputs, saturation_status &sat_status,
+					float min_output, float max_output) const;
+
+	/**
+	 * Minimize the saturation of the actuators by adding or substracting a fraction of delta_outputs.
+	 * delta_outputs is the vector that added to the output outputs, modifies the thrust or angular
+	 * acceleration on a specific axis.
+	 * For example, if delta_outputs is given to slide along the vertical thrust axis (thrust_scale), the
+	 * saturation will be minimized by shifting the vertical thrust setpoint, without changing the
+	 * roll/pitch/yaw accelerations.
+	 *
+	 * Note that as we only slide along the given axis, in extreme cases outputs can still contain values
+	 * outside of [min_output, max_output].
+	 *
+	 * @param delta_outputs vector that is added to the outputs, e.g. thrust_scale
+	 * @param outputs output vector that is modified
+	 * @param sat_status saturation status output
+	 * @param min_output minimum desired value in outputs
+	 * @param max_output maximum desired value in outputs
+	 * @param reduce_only if true, only allow to reduce (substract) a fraction of delta_outputs
+	 */
+	void minimize_saturation(const float *delta_outputs, float *outputs, saturation_status &sat_status,
+				 float min_output = 0.f, float max_output = 1.f, bool reduce_only = false) const;
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: airmode for roll/pitch:
+	 * thrust is increased/decreased as much as required to meet the demanded roll/pitch.
+	 * Yaw is not allowed to increase the thrust, @see mix_yaw() for the exact behavior.
+	 */
+	inline void mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: full airmode for roll/pitch/yaw:
+	 * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw.
+	 */
+	inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix roll, pitch, yaw, thrust and set the outputs vector.
+	 *
+	 * Desaturation behavior: no airmode, thrust is NEVER increased to meet the demanded
+	 * roll/pitch/yaw. Instead roll/pitch/yaw is reduced as much as needed.
+	 * Thrust can be reduced to unsaturate the upper side.
+	 * @see mix_yaw() for the exact yaw behavior.
+	 */
+	inline void mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs);
+
+	/**
+	 * Mix yaw by updating an existing output vector (that already contains roll/pitch/thrust).
+	 *
+	 * Desaturation behavior: thrust is allowed to be decreased up to 15% in order to allow
+	 * some yaw control on the upper end. On the lower end thrust will never be increased,
+	 * but yaw is decreased as much as required.
+	 *
+	 * @param yaw demanded yaw
+	 * @param outputs output vector that is updated
+	 */
+	inline void mix_yaw(float yaw, float *outputs);
+
+	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);
+
 	float				_roll_scale;
 	float				_pitch_scale;
 	float				_yaw_scale;
@@ -702,13 +774,13 @@ private:
 
 	Airmode				_airmode;
 
-	void update_saturation_status(unsigned index, bool clipping_high, bool clipping_low);
 	saturation_status _saturation_status;
 
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
 
 	float 				*_outputs_prev = nullptr;
+	float 				*_tmp_array = nullptr;
 
 	/* do not allow to copy due to ptr data members */
 	MultirotorMixer(const MultirotorMixer &);
diff --git a/src/lib/mixer/mixer_multirotor.cpp b/src/lib/mixer/mixer_multirotor.cpp
index 5ff9ec1f45..bd749fdf52 100644
--- a/src/lib/mixer/mixer_multirotor.cpp
+++ b/src/lib/mixer/mixer_multirotor.cpp
@@ -53,11 +53,6 @@
 //#include <debug.h>
 //#define debug(fmt, args...)	syslog(fmt "\n", ##args)
 
-/*
- * Clockwise: 1
- * Counter-clockwise: -1
- */
-
 MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 				 uintptr_t cb_handle,
 				 MultirotorGeometry geometry,
@@ -75,7 +70,8 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 	_airmode(Airmode::disabled),
 	_rotor_count(_config_rotor_count[(MultirotorGeometryUnderlyingType)geometry]),
 	_rotors(_config_index[(MultirotorGeometryUnderlyingType)geometry]),
-	_outputs_prev(new float[_rotor_count])
+	_outputs_prev(new float[_rotor_count]),
+	_tmp_array(new float[_rotor_count])
 {
 	for (unsigned i = 0; i < _rotor_count; ++i) {
 		_outputs_prev[i] = _idle_speed;
@@ -84,9 +80,8 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 
 MultirotorMixer::~MultirotorMixer()
 {
-	if (_outputs_prev != nullptr) {
-		delete[] _outputs_prev;
-	}
+	delete[] _outputs_prev;
+	delete[] _tmp_array;
 }
 
 MultirotorMixer *
@@ -146,146 +141,192 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 		       s[3] / 10000.0f);
 }
 
+float MultirotorMixer::compute_desaturation_gain(const float *delta_outputs, const float *outputs,
+		saturation_status &sat_status, float min_output, float max_output) const
+{
+	float k_min = 0.f;
+	float k_max = 0.f;
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		// Avoid division by zero. If delta_outputs[i] is zero, there's nothing we can do to unsaturate anyway
+		if (fabsf(delta_outputs[i]) < FLT_EPSILON) {
+			continue;
+		}
+
+		if (outputs[i] < min_output) {
+			float k = (min_output - outputs[i]) / delta_outputs[i];
+
+			if (k < k_min) { k_min = k; }
+
+			if (k > k_max) { k_max = k; }
+
+			sat_status.flags.motor_neg = true;
+		}
+
+		if (outputs[i] > max_output) {
+			float k = (max_output - outputs[i]) / delta_outputs[i];
+
+			if (k < k_min) { k_min = k; }
+
+			if (k > k_max) { k_max = k; }
+
+			sat_status.flags.motor_pos = true;
+		}
+	}
+
+	// Reduce the saturation as much as possible
+	return k_min + k_max;
+}
+
+void MultirotorMixer::minimize_saturation(const float *delta_outputs, float *outputs, saturation_status &sat_status,
+		float min_output, float max_output, bool reduce_only) const
+{
+	float k1 = compute_desaturation_gain(delta_outputs, outputs, sat_status, min_output, max_output);
+
+	if (reduce_only && k1 > 0.f) {
+		return;
+	}
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] += k1 * delta_outputs[i];
+	}
+
+	// Compute the desaturation gain again based on the updated outputs.
+	// In most cases it will be zero. It won't be if max(outputs) - min(outputs) > max_output - min_output.
+	// In that case adding 0.5 of the gain will equilibrate saturations.
+	float k2 = 0.5f * compute_desaturation_gain(delta_outputs, outputs, sat_status, min_output, max_output);
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] += k2 * delta_outputs[i];
+	}
+}
+
+void MultirotorMixer::mix_airmode_rp(float roll, float pitch, float yaw, float thrust, float *outputs)
+{
+	// Airmode for roll and pitch, but not yaw
+
+	// Mix without yaw
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors[i].roll_scale +
+			     pitch * _rotors[i].pitch_scale +
+			     thrust * _rotors[i].thrust_scale;
+
+		// Thrust will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Mix yaw independently
+	mix_yaw(yaw, outputs);
+}
+
+void MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs)
+{
+	// Airmode for roll, pitch and yaw
+
+	// Do full mixing
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors[i].roll_scale +
+			     pitch * _rotors[i].pitch_scale +
+			     yaw * _rotors[i].yaw_scale +
+			     thrust * _rotors[i].thrust_scale;
+
+		// Thrust will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+}
+
+void MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs)
+{
+	// Airmode disabled: never allow to increase the thrust to unsaturate a motor
+
+	// Mix without yaw
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = roll * _rotors[i].roll_scale +
+			     pitch * _rotors[i].pitch_scale +
+			     thrust * _rotors[i].thrust_scale;
+
+		// Thrust will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	// only reduce thrust
+	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.f, true);
+
+	// Reduce roll/pitch acceleration if needed to unsaturate
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].roll_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].pitch_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Mix yaw independently
+	mix_yaw(yaw, outputs);
+}
+
+void MultirotorMixer::mix_yaw(float yaw, float *outputs)
+{
+	// Add yaw to outputs
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] += yaw * _rotors[i].yaw_scale;
+
+		// Yaw will be used to unsaturate if needed
+		_tmp_array[i] = _rotors[i].yaw_scale;
+	}
+
+	// Change yaw acceleration to unsaturate the outputs if needed (do not change roll/pitch),
+	// and allow some yaw response at maximum thrust
+	minimize_saturation(_tmp_array, outputs, _saturation_status, 0.f, 1.15f);
+
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].thrust_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status, -1000.f);
+}
+
 unsigned
 MultirotorMixer::mix(float *outputs, unsigned space)
 {
-	/* Summary of mixing strategy:
-	1) mix roll, pitch and thrust without yaw.
-	2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
-		increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
-		(max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
-		In case there is violation at the lower and upper bound then try to shift such that violation is equal
-		on both sides.
-	3) mix in yaw and scale if it leads to limit violation.
-	4) scale all outputs to range [idle_speed,1]
-	*/
-
-	float		roll    = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
-	float		pitch   = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
-	float		yaw     = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
-	float		thrust  = math::constrain(get_control(0, 3), 0.0f, 1.0f);
-	float		min_out = 1.0f;
-	float		max_out = 0.0f;
+	float roll    = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
+	float pitch   = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
+	float yaw     = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
+	float thrust  = math::constrain(get_control(0, 3), 0.0f, 1.0f);
 
 	// clean out class variable used to capture saturation
 	_saturation_status.value = 0;
 
-	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
+	// Do the mixing using the strategy given by the current Airmode configuration
+	switch (_airmode) {
+	case Airmode::roll_pitch:
+		mix_airmode_rp(roll, pitch, yaw, thrust, outputs);
+		break;
+
+	case Airmode::roll_pitch_yaw:
+		mix_airmode_rpy(roll, pitch, yaw, thrust, outputs);
+		break;
+
+	case Airmode::disabled:
+	default: // just in case: default to disabled
+		mix_airmode_disabled(roll, pitch, yaw, thrust, outputs);
+		break;
+	}
+
+	// Apply thrust model and scale outputs to range [idle_speed, 1].
+	// At this point the outputs are expected to be in [0, 1], but they can be outside, for example
+	// if a roll command exceeds the motor band limit.
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		float out = roll * _rotors[i].roll_scale +
-			    pitch * _rotors[i].pitch_scale +
-			    thrust * _rotors[i].thrust_scale;
-
-		/* calculate min and max output values */
-		if (out < min_out) {
-			min_out = out;
-		}
-
-		if (out > max_out) {
-			max_out = out;
-		}
-
-		outputs[i] = out;
-	}
-
-	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_scale==1 for all motors.
-	// On asymmetric platforms, some motors have thrust_scale<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;
-
-		} else if (max_out > 1.0f) {
-			boost = -(max_out - 1.0f);
-		}
-
-	} else {
-		roll_pitch_scale = 1.0f / (delta_out_max);
-		boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust);
-	}
-
-	if (_airmode == Airmode::disabled) {
-		// disable positive boosting if not in air-mode
-		// boosting can only be positive when min_out < 0.0
-		// roll_pitch_scale is reduced accordingly
-		if (boost > 0.0f) {
-			roll_pitch_scale = thrust / (thrust - min_out);
-			boost = 0.0f;
-		}
-	}
-
-	// capture saturation
-	if (min_out < 0.0f) {
-		_saturation_status.flags.motor_neg = true;
-	}
-
-	if (max_out > 1.0f) {
-		_saturation_status.flags.motor_pos = true;
-	}
-
-	// Thrust reduction is used to reduce the collective thrust if we hit
-	// the upper throttle limit
-	float thrust_reduction = 0.0f;
-
-	// mix again but now with thrust boost, scale roll/pitch and also add yaw
-	for (unsigned i = 0; i < _rotor_count; i++) {
-		float out = (roll * _rotors[i].roll_scale +
-			     pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
-			    yaw * _rotors[i].yaw_scale +
-			    (thrust + boost) * _rotors[i].thrust_scale;
-
-		// scale yaw if it violates limits. inform about yaw limit reached
-		if (out < 0.0f) {
-			if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
-				yaw = 0.0f;
-
-			} else {
-				yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
-					roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
-			}
-
-		} else if (out > 1.0f) {
-			// allow to reduce thrust to get some yaw response
-			float prop_reduction = fminf(0.15f, out - 1.0f);
-			// keep the maximum requested reduction
-			thrust_reduction = fmaxf(thrust_reduction, prop_reduction);
-
-			if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
-				yaw = 0.0f;
-
-			} else {
-				yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
-					       roll_pitch_scale + (thrust - thrust_reduction) + boost)) / _rotors[i].yaw_scale;
-			}
-		}
-	}
-
-	// Apply collective thrust reduction, the maximum for one prop
-	thrust -= thrust_reduction;
-
-	// add yaw and scale outputs to range idle_speed...1
-	for (unsigned i = 0; i < _rotor_count; i++) {
-		outputs[i] = (roll * _rotors[i].roll_scale +
-			      pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
-			     yaw * _rotors[i].yaw_scale +
-			     (thrust + boost) * _rotors[i].thrust_scale;
-
-		/*
-			implement simple model for static relationship between applied motor pwm and motor thrust
-			model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
-			this model assumes normalized input / output in the range [0,1] so this is the right place
-			to do it as at this stage the outputs are in that range.
-		 */
+		// Implement simple model for static relationship between applied motor pwm and motor thrust
+		// model: thrust = (1 - _thrust_factor) * PWM + _thrust_factor * PWM^2
 		if (_thrust_factor > 0.0f) {
 			outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) + sqrtf((1.0f - _thrust_factor) *
 					(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) + (outputs[i] < 0.0f ? 0.0f : outputs[i] /
@@ -293,10 +334,9 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		}
 
 		outputs[i] = math::constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
-
 	}
 
-	/* slew rate limiting and saturation checking */
+	// Slew rate limiting and saturation checking
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		bool clipping_high = false;
 		bool clipping_low = false;