mc mixer: prioritize roll/pitch over yaw for full airmode

Improves roll/pitch tracking in situations of large yaw demands.

src/lib/mixer/mixer.h

@@ -773,7 +773,8 @@ private:
 	 * 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.
+	 * thrust is increased/decreased as much as required to meet demanded the roll/pitch/yaw,
+	 * while giving priority to roll and pitch over yaw.
 	 */
 	inline void mix_airmode_rpy(float roll, float pitch, float yaw, float thrust, float *outputs);
 

src/lib/mixer/mixer_multirotor.cpp

@@ -285,6 +285,14 @@ void MultirotorMixer::mix_airmode_rpy(float roll, float pitch, float yaw, float
 	}
 
 	minimize_saturation(_tmp_array, outputs, _saturation_status);
+
+	// Unsaturate yaw (in case upper and lower bounds are exceeded)
+	// to prioritize roll/pitch over yaw.
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		_tmp_array[i] = _rotors[i].yaw_scale;
+	}
+
+	minimize_saturation(_tmp_array, outputs, _saturation_status);
 }
 
 void MultirotorMixer::mix_airmode_disabled(float roll, float pitch, float yaw, float thrust, float *outputs)

src/lib/mixer/mixer_multirotor.py

@@ -134,7 +134,13 @@ def airmode_rpy(m_sp, P, u_min, u_max):
     # Use thrust to unsaturate the outputs if needed
     u_T = P[:, 3]
     u_prime = minimize_sat(u, u_min, u_max, u_T)
-    return (u, u_prime)
+
+    # Unsaturate yaw (in case upper and lower bounds are exceeded)
+    # to prioritize roll/pitch over yaw.
+    u_T = P[:, 2]
+    u_prime_yaw = minimize_sat(u_prime, u_min, u_max, u_T)
+
+    return (u, u_prime_yaw)
 
 
 def normal_mode(m_sp, P, u_min, u_max):