mc_att_control: fix computation of yaw weight for attitude control

Previously, the yaw weight was based on the tilt angle of the attitude
setpoint (R_sp(2, 2) == cos(tilt angle)). This makes no sense, it means
the weight is low for high tilt angles even if there is no roll-pitch error
at all.

This patch changes the weight computation to be based on the tilt angle
error: the yaw weight is 1 if there is no roll-pitch error (independent
from current tilt angle), and is reduced for higher tilt angle errors.

The weight was added in 05e9a30573, without any explanation or
derivation of how and why the weight is chosen that way.

However this patch works much better in practice. The yaw control is
improved, though it can be a bit slow to converge if you do continuous &
fast roll-pitch motions (which is expected).

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -882,9 +882,6 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	float e_R_z_sin = e_R.length(); // == sin(tilt angle error)
 	float e_R_z_cos = R_z * R_sp_z; // == cos(tilt angle error) == (R.transposed() * R_sp)(2, 2)
 
-	/* calculate weight for yaw control */
-	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
-
 	/* calculate rotation matrix after roll/pitch only rotation */
 	math::Matrix<3, 3> R_rp;
 
@@ -916,6 +913,14 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	/* R_rp and R_sp have the same Z axis, calculate yaw error */
 	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
 	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+
+	/* calculate the weight for yaw control
+	 * Make the weight depend on the tilt angle error: the higher the error of roll and/or pitch, the lower
+	 * the weight that we use to control the yaw. This gives precedence to roll & pitch correction.
+	 * The weight is 1 if there is no tilt error.
+	 */
+	float yaw_w = e_R_z_cos * e_R_z_cos;
+
 	/* calculate the angle between R_rp_x and R_sp_x (yaw angle error), and apply the yaw weight */
 	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;