mc_att_control: add reduced quaternion attitude control

to prioritize yaw compared to roll and pitch by combining
the shortest rotation to achieve a total thrust vector with
the full attitude respecting the desired yaw
not by scaling down the control output with the gains

src/lib/matrix

@@ -1 +1 @@
-Subproject commit f4243160e2c77eb8034a35fe42924d59a39319da
+Subproject commit 41a1cc7583253f3063ab4c626c40e7bc2a0bc868

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -849,7 +849,20 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	q.normalize();
 	qd.normalize();
 
-	/* full quaternion attitude control, qe is rotation from q to qd */
+
+	/* calculate reduced attitude which we would command if we about the vehicle's yaw */
+	Vector3f e_z = q.dcm_z();
+	Vector3f e_z_d = qd.dcm_z();
+	Quatf qd_red(e_z, e_z_d);
+	qd_red *= q;
+
+	/* mix full and reduced desired attitude */
+	Quatf q_mix = qd_red.inversed() * qd;
+	q_mix *= math::sign(q_mix(0));
+	qd = qd_red * Quatf(cosf(yaw_w * acosf(q_mix(0))), 0, 0, sinf(yaw_w * asinf(q_mix(3))));
+
+
+	/* quaternion attitude control law, qe is rotation from q to qd */
 	Quatf qe = q.inversed() * qd;
 
 	/* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)