[feature] ahrs_dcm_float uses magnetic heading while not inflight for better initial guess

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -16,6 +16,11 @@
  *  \brief Attitude estimation for fixedwings based on the DCM
  *  Theory: http://code.google.com/p/gentlenav/downloads/list  file DCMDraft2.pdf
  *
+ *  Options:
+ *  -USE_HIGH_ACCEL_FLAG: no compensation when high accelerations present
+ *  -USE_MAGNETOMETER_ONGROUND: use magnetic compensation before takeoff only while GPS course not good
+ *  -USE_AHRS_GPS_ACCELERATIONS: forward acceleration compensation from GPS speed
+ *
  */
 
 #include "std.h"
@@ -25,6 +30,7 @@
 #include "subsystems/ahrs/ahrs_float_utils.h"
 #include "subsystems/ahrs/ahrs_aligner.h"
 #include "subsystems/imu.h"
+#include "firmwares/fixedwing/autopilot.h"	// launch detection
 
 #include "subsystems/ahrs/ahrs_float_dcm_algebra.h"
 #include "math/pprz_algebra_float.h"
@@ -492,6 +498,21 @@ void Drift_correction(void)
     Vector_Scale(&Scaled_Omega_I[0],&errorYaw[0],Ki_YAW);
     Vector_Add(Omega_I,Omega_I,Scaled_Omega_I);//adding integrator to the Omega_I
   }
+#if USE_MAGNETOMETER_ONGROUND == 1
+#pragma message AHRS_FLOAT_DCM uses magnetometer prior to takeoff and GPS during flight
+  else if (launch == FALSE)
+  {
+    float COGX = imu.mag.x; // Non-Tilt-Compensated (for filter stability reasons)
+    float COGY = imu.mag.y; // Non-Tilt-Compensated (for filter stability reasons)
+
+    errorCourse=(DCM_Matrix[0][0]*COGY) - (DCM_Matrix[1][0]*COGX);  //Calculating YAW error
+    Vector_Scale(errorYaw,&DCM_Matrix[2][0],errorCourse); //Applys the yaw correction to the XYZ rotation of the aircraft, depeding the position.
+
+    // P only
+    Vector_Scale(&Scaled_Omega_P[0],&errorYaw[0],Kp_YAW / 10.0);
+    Vector_Add(Omega_P,Omega_P,Scaled_Omega_P);//Adding  Proportional.fi
+  }
+#endif // USE_MAGNETOMETER_ONGROUND
 #endif
 
   //  Here we will place a limit on the integrator so that the integrator cannot ever exceed half the saturation limit of the gyros