[ahrs] use quaternion to compute initial DCM orientation

the euler formula is not correct when the IMU is upside-down

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -117,14 +117,20 @@ void ahrs_dcm_init(void)
 }
 
 bool ahrs_dcm_align(struct FloatRates *lp_gyro, struct FloatVect3 *lp_accel,
-                      struct FloatVect3 *lp_mag)
+                      struct FloatVect3 *lp_mag __attribute__((unused)))
 {
-  /* Compute an initial orientation using euler angles */
-  ahrs_float_get_euler_from_accel_mag(&ahrs_dcm.ltp_to_imu_euler, lp_accel, lp_mag);
+  /* Compute an initial orientation in quaternion (and then back to euler angles) so it works upside-down as well */
+  struct FloatQuat quat;
+#if USE_MAGNETOMETER
+  ahrs_float_get_quat_from_accel_mag(&quat, lp_accel, lp_mag);
+#else
+  ahrs_float_get_quat_from_accel(&quat, lp_accel);
+#endif
+  float_eulers_of_quat(&ahrs_dcm.ltp_to_imu_euler, &quat);
 
-  /* Convert initial orientation in quaternion and rotation matrice representations. */
+  /* Convert initial orientation from quaternion to rotation matrix representations. */
   struct FloatRMat ltp_to_imu_rmat;
-  float_rmat_of_eulers(&ltp_to_imu_rmat, &ahrs_dcm.ltp_to_imu_euler);
+  float_rmat_of_quat(&ltp_to_imu_rmat, &quat);
 
   /* set filter dcm */
   set_dcm_matrix_from_rmat(&ltp_to_imu_rmat);

sw/airborne/subsystems/ahrs/ahrs_float_utils.h

@@ -35,6 +35,8 @@
 
 #include "std.h" // for ABS
 
+/** Computer orientation in euler angles from accel and mag
+ *  This is not working when the IMU is upside-down, then use the quaternion based function */
 static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers *e, struct FloatVect3 *accel,
     struct FloatVect3 *mag)
 {