ahrs_int_cmpl_quat: yaw update via heading measurement

- added ahrs_update_heading which gets called from ahrs_update_gps with gps course
- define AHRS_USE_GPS_HEADING in your airframe file to use it

conf/airframes/fraser_lisa_m_rotorcraft.xml

@@ -25,7 +25,7 @@
     <subsystem name="imu"           type="aspirin_v1.5"/>
     <subsystem name="gps"           type="ublox"/>
     <subsystem name="stabilization" type="euler"/>
-    <subsystem name="ahrs"          type="int_cmpl_euler"/>
+    <subsystem name="ahrs"          type="int_cmpl_quat"/>
   </firmware>
 
   <firmware name="lisa_m_test_progs">

sw/airborne/subsystems/ahrs/ahrs_int_cmpl.c

@@ -23,7 +23,6 @@
 // TODO
 //
 // gravity heuristic
-// gps update for yaw on fixed wing ?
 //
 
 #include "subsystems/ahrs/ahrs_int_cmpl.h"
@@ -321,6 +320,60 @@ void ahrs_update_gps(void) {
     ahrs_impl.ltp_vel_norm_valid = FALSE;
   }
 #endif
+
+#if AHRS_USE_GPS_HEADING && USE_GPS
+  //got a 3d fix and ground speed is more than 0.5 m/s
+  if(gps.fix == GPS_FIX_3D && gps.gspeed>= 500) {
+    // gps.course is in rad * 1e7, we need it in rad * 2^INT32_ANGLE_FRAC
+    int32_t course = gps.course * ((1<<INT32_ANGLE_FRAC) / 1e7);
+    ahrs_update_course(course);
+  }
+#endif
+}
+
+/** Update yaw based on a heading measurement.
+ * e.g. from GPS course
+ * @param heading Heading in radians (CW/north) with #INT32_ANGLE_FRAC
+ */
+void ahrs_update_heading(int32_t heading) {
+
+  INT32_ANGLE_NORMALIZE(heading);
+
+  // row 0 of ltp_to_body_rmat = body x-axis in ltp frame
+  // we only consider x and y
+  struct Int32Vect2 expected_ltp =
+    { RMAT_ELMT(ahrs.ltp_to_body_rmat, 0, 0),
+      RMAT_ELMT(ahrs.ltp_to_body_rmat, 0, 1) };
+
+  int32_t heading_x, heading_y;
+  PPRZ_ITRIG_COS(heading_x, heading); // measured course in x-direction
+  PPRZ_ITRIG_SIN(heading_y, heading); // measured course in y-direction
+
+  // expected_heading cross measured_heading ??
+  struct Int32Vect3 residual_ltp =
+    { 0,
+      0,
+      (expected_ltp.x * heading_y - expected_ltp.y * heading_x)/(1<<INT32_ANGLE_FRAC)};
+
+  struct Int32Vect3 residual_imu;
+  INT32_RMAT_VMULT(residual_imu, ahrs.ltp_to_imu_rmat, residual_ltp);
+
+  // residual FRAC = TRIG_FRAC + TRIG_FRAC = 14 + 14 = 28
+  // rate_correction FRAC = RATE_FRAC = 12
+  // 2^12 / 2^28 * 4.0 = 1/2^14
+  // (1<<INT32_ANGLE_FRAC)/2^14 = 1/4
+  ahrs_impl.rate_correction.p += residual_imu.x/4;
+  ahrs_impl.rate_correction.q += residual_imu.y/4;
+  ahrs_impl.rate_correction.r += residual_imu.z/4;
+
+  // residual_ltp FRAC = 2 * TRIG_FRAC = 28
+  // high_rez_bias = RATE_FRAC+28 = 40
+  // 2^40 / 2^28 * 2.5e-4 = 1
+  ahrs_impl.high_rez_bias.p -= residual_imu.x*(1<<INT32_ANGLE_FRAC);
+  ahrs_impl.high_rez_bias.q -= residual_imu.y*(1<<INT32_ANGLE_FRAC);
+  ahrs_impl.high_rez_bias.r -= residual_imu.z*(1<<INT32_ANGLE_FRAC);
+
+  INT_RATES_RSHIFT(ahrs_impl.gyro_bias, ahrs_impl.high_rez_bias, 28);
 }
 
 /* Compute ltp to imu rotation in quaternion and rotation matrice representation

sw/airborne/subsystems/ahrs/ahrs_int_cmpl.h

@@ -38,6 +38,7 @@ struct AhrsIntCmpl {
 
 extern struct AhrsIntCmpl ahrs_impl;
 
+void ahrs_update_heading(int32_t heading);
 
 #ifdef AHRS_UPDATE_FW_ESTIMATOR
 // TODO copy ahrs to state instead of estimator

sw/airborne/subsystems/gps.h

@@ -68,7 +68,7 @@ struct GpsState {
   struct NedCoor_i ned_vel;      ///< speed NED in cm/s
   int16_t gspeed;                ///< norm of 2d ground speed in cm/s
   int16_t speed_3d;              ///< norm of 3d speed in cm/s
-  int32_t course;                ///< GPS heading in rad*1e7
+  int32_t course;                ///< GPS heading in rad*1e7 (CW/north)
   uint32_t pacc;                 ///< position accuracy
   uint32_t sacc;                 ///< speed accuracy
   uint16_t pdop;                 ///< dilution of precision