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.