ahrs cmpl: directly compute initial quaternion from accel and mag instead of using euler angles

closes #132

conf/airframes/fraser_lisa_m_rotorcraft.xml

@@ -25,6 +25,9 @@
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization" type="euler"/>
     <subsystem name="ahrs"          type="int_cmpl_quat"/>
+    <!--subsystem name="ahrs"          type="float_cmpl">
+      <define name="AHRS_PROPAGATE_QUAT"/>
+    </subsystem-->
   </firmware>
 
   <firmware name="setup">
@@ -55,7 +58,7 @@
   </firmware>
 
   <modules main_freq="512">
-    <!--load name="sys_mon.xml"/-->
+    <load name="sys_mon.xml"/>
   </modules>
 
   <commands>

sw/airborne/subsystems/ahrs/ahrs_float_cmpl.c

@@ -85,14 +85,27 @@ void ahrs_init(void) {
 
 }
 
+#define AHRS_ALIGN_QUAT 1
 
 void ahrs_align(void) {
 
+#if AHRS_ALIGN_QUAT
+
+  /* Compute an initial orientation from accel and mag directly as quaternion */
+  ahrs_float_get_quat_from_accel_mag(&ahrs_float.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
+  /* Convert initial orientation from quat to euler and rotation matrix representations. */
+  compute_imu_rmat_and_euler_from_quat();
+
+#else
+
   /* Compute an initial orientation using euler angles */
   ahrs_float_get_euler_from_accel_mag(&ahrs_float.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
-  /* Convert initial orientation in quaternion and rotation matrice representations. */
+  /* Convert initial orientation in quaternion and rotation matrix representations. */
   FLOAT_QUAT_OF_EULERS(ahrs_float.ltp_to_imu_quat, ahrs_float.ltp_to_imu_euler);
   FLOAT_RMAT_OF_QUAT(ahrs_float.ltp_to_imu_rmat, ahrs_float.ltp_to_imu_quat);
+
+#endif
+
   /* Compute initial body orientation */
   compute_body_orientation_and_rates();
 

sw/airborne/subsystems/ahrs/ahrs_float_utils.h

@@ -3,6 +3,8 @@
 
 #include "subsystems/ahrs/ahrs_magnetic_field_model.h"
 
+#define ABS(_x) ((_x) < 0 ? -(_x) : (_x))
+
 static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
   /* get phi and theta from accelerometer */
   struct FloatVect3 accelf;
@@ -26,4 +28,69 @@ static inline void ahrs_float_get_euler_from_accel_mag(struct FloatEulers* e, st
 
 }
 
+static inline void ahrs_float_get_quat_from_accel_mag(struct FloatQuat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
+
+  /* normalized accel measurement in floating point */
+  struct FloatVect3 acc_normalized;
+  ACCELS_FLOAT_OF_BFP(acc_normalized, *accel);
+  FLOAT_VECT3_NORMALIZE(acc_normalized);
+
+  /* the quaternion representing roll and pitch from acc measurement */
+  struct FloatQuat q_a;
+
+  /*
+   * axis we want to rotate around is cross product of accel and reference [0,0,-g]
+   * normalized: cross(acc_normalized, [0,0,-1])
+   * vector part of quaternion is the axis
+   * scalar part (angle): 1.0 + dot(acc_normalized, [0,0,-1])
+   */
+  q_a.qx = - acc_normalized.y;
+  q_a.qy = acc_normalized.x;
+  q_a.qz = 0.0;
+  q_a.qi = 1.0 - acc_normalized.z;
+  FLOAT_QUAT_NORMALIZE(q_a);
+
+  /* handle 180deg case */
+  if ( ABS(acc_normalized.z - 1.0) < 5*FLT_MIN ) {
+    QUAT_ASSIGN(q_a, 0.0, 1.0, 0.0, 0.0);
+  }
+
+
+  /* convert mag measurement to float */
+  struct FloatVect3 mag_float;
+  MAGS_FLOAT_OF_BFP(mag_float, *mag);
+
+  /* and rotate to horizontal plane using the quat from above */
+  struct FloatRMat rmat_phi_theta;
+  FLOAT_RMAT_OF_QUAT(rmat_phi_theta, q_a);
+  struct FloatVect3 mag_ltp;
+  FLOAT_RMAT_VECT3_TRANSP_MUL(mag_ltp, rmat_phi_theta, mag_float);
+
+  /* heading from mag -> make quaternion to rotate around ltp z axis*/
+  struct FloatQuat q_m;
+
+  /* dot([mag_n.x, mag_n.x, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
+  float dot = mag_ltp.x * AHRS_H_X + mag_ltp.y * AHRS_H_Y;
+
+  /* |v1||v2| */
+  float norm2 = sqrtf(SQUARE(mag_ltp.x) + SQUARE(mag_ltp.y))
+    * sqrtf(SQUARE(AHRS_H_X) + SQUARE(AHRS_H_Y));
+
+  // catch 180deg case
+  if (ABS(norm2 + dot) < 5*FLT_MIN) {
+    QUAT_ASSIGN(q_m, 0.0, 0.0, 0.0, 1.0);
+  } else {
+    /* q_xyz = cross([mag_n.x, mag_n.y, 0], [AHRS_H_X, AHRS_H_Y, 0]) */
+    q_m.qx = 0.0;
+    q_m.qy = 0.0;
+    q_m.qz = mag_ltp.x * AHRS_H_Y - mag_ltp.y * AHRS_H_X;
+    q_m.qi = norm2 + dot;
+    FLOAT_QUAT_NORMALIZE(q_m);
+  }
+
+  // q_ltp2imu = q_a * q_m
+  // and wrap and normalize
+  FLOAT_QUAT_COMP_NORM_SHORTEST(*q, q_m, q_a);
+}
+
 #endif /* AHRS_FLOAT_UTILS_H */

sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c

@@ -112,10 +112,10 @@ void ahrs_init(void) {
 
 void ahrs_align(void) {
 
-  /* Compute an initial orientation using euler angles */
+  /* Compute an initial orientation from accel and mag directly as quaternion */
-  ahrs_int_get_euler_from_accel_mag(&ahrs.ltp_to_imu_euler, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
+  ahrs_int_get_quat_from_accel_mag(&ahrs.ltp_to_imu_quat, &ahrs_aligner.lp_accel, &ahrs_aligner.lp_mag);
-  /* Convert initial orientation in quaternion and rotation matrice representations. */
+  /* Convert initial orientation from quat to euler and rotation matrix representations. */
-  compute_imu_quat_and_rmat_from_euler();
+  compute_imu_euler_and_rmat_from_quat();
 
   compute_body_orientation();
 
@@ -376,16 +376,6 @@ void ahrs_update_heading(int32_t heading) {
   INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
 }
 
-/* Compute ltp to imu rotation in quaternion and rotation matrice representation
-   from the euler angle representation */
-__attribute__ ((always_inline)) static inline void compute_imu_quat_and_rmat_from_euler(void) {
-
-  /* Compute LTP to IMU quaternion */
-  INT32_QUAT_OF_EULERS(ahrs.ltp_to_imu_quat, ahrs.ltp_to_imu_euler);
-  /* Compute LTP to IMU rotation matrix */
-  INT32_RMAT_OF_EULERS(ahrs.ltp_to_imu_rmat, ahrs.ltp_to_imu_euler);
-
-}
 
 /* Compute ltp to imu rotation in euler angles and rotation matrice representation
    from the quaternion representation */

sw/airborne/subsystems/ahrs/ahrs_int_utils.h

@@ -6,6 +6,8 @@
 
 #include "subsystems/ahrs/ahrs_magnetic_field_model.h"
 
+#include "subsystems/ahrs/ahrs_float_utils.h"
+
 static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, struct Int32Vect3* accel, struct Int32Vect3* mag) {
   //  DISPLAY_INT32_VECT3("# accel", (*accel));
   const float fphi = atan2f(-accel->y, -accel->z);
@@ -44,4 +46,11 @@ static inline void ahrs_int_get_euler_from_accel_mag(struct Int32Eulers* e, stru
 
 }
 
+static inline void ahrs_int_get_quat_from_accel_mag(struct Int32Quat* q, struct Int32Vect3* accel, struct Int32Vect3* mag) {
+
+  struct FloatQuat q_f;
+  ahrs_float_get_quat_from_accel_mag(&q_f, accel, mag);
+  QUAT_BFP_OF_REAL(*q, q_f);
+}
+
 #endif /* AHRS_INT_UTILS_H */