mc_att_control: catch numerical corner cases

- Delete left over identity matrix.

- Corner case with a zero element when using the signum function:
We always need a sign also for zero.

- Corner case with arbitrary yaw but and 180 degree roll or pitch:
Reduced attitude control calculation always rotates around roll
because there's no right choice when neglecting yaw. In that small
corner case it's better to just use full attitude contol and hence
rotate around the most efficient roll/pitch combination.

src/lib/mathlib/math/Functions.hpp

@@ -51,6 +51,13 @@ int sign(T val)
 	return (T(0) < val) - (val < T(0));
 }
 
+// Type-safe signum function with zero treted as positive
+template<typename T>
+int signNoZero(T val)
+{
+	return (T(0) <= val) - (val < T(0));
+}
+
 /*
  * So called exponential curve function implementation.
  * It is essentially a linear combination between a linear and a cubic function.

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -182,8 +182,6 @@ private:
 	float			_thrust_sp;		/**< thrust setpoint */
 	math::Vector<3>		_att_control;	/**< attitude control vector */
 
-	math::Matrix<3, 3>  _I;				/**< identity matrix */
-
 	math::Matrix<3, 3>	_board_rotation = {};	/**< rotation matrix for the orientation that the board is mounted */
 
 	struct {
@@ -420,7 +418,6 @@ _loop_update_rate_hz(initial_update_rate_hz)
 	_thrust_sp = 0.0f;
 	_att_control.zero();
 
-	_I.identity();
 	_board_rotation.identity();
 
 	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
@@ -842,15 +839,22 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	q.normalize();
 	qd.normalize();
 
-	/* calculate reduced attitude which we would command if we about the vehicle's yaw */
+	/* calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch */
 	Vector3f e_z = q.dcm_z();
 	Vector3f e_z_d = qd.dcm_z();
 	Quatf qd_red(e_z, e_z_d);
-	qd_red *= q;
+
+	if (Quatf(qd_red - Quatf(0, 1, 0, 0)).norm() > 1e-5f) {
+		qd_red *= q;
+
+	} else {
+		/* except for the numerical corner case where it doesn't help */
+		qd_red = qd;
+	}
 
 	/* mix full and reduced desired attitude */
 	Quatf q_mix = qd_red.inversed() * qd;
-	q_mix *= math::sign(q_mix(0));
+	q_mix *= math::signNoZero(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 */
@@ -858,13 +862,12 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 	/* using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)
 	 * also taking care of the antipodal unit quaternion ambiguity */
-	Vector3f eq = 2.f * math::sign(qe(0)) * qe.imag();
+	Vector3f eq = 2.f * math::signNoZero(qe(0)) * qe.imag();
 
 	/* calculate angular rates setpoint */
 	eq = eq.emult(attitude_gain);
 	_rates_sp = math::Vector<3>(eq.data());
 
-
 	/* Feed forward the yaw setpoint rate. We need to transform the yaw from world into body frame.
 	 * The following is a simplification of:
 	 * R.transposed() * Vector3f(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff)