[ahrs] add filter on accel heuristic of DCM AHRS

the same filter is already applied in the int_quat ahrs and can prevent
incorrect measurement rejection when strong structural vibrations
increase the IMU noise

conf/modules/ahrs_float_dcm.xml

@@ -18,6 +18,7 @@
     <configure name="AHRS_ALIGNER_LED" value="1" description="LED number to indicate AHRS alignment, none to disable (default is board dependent)"/>
     <define name="USE_MAGNETOMETER_ONGROUND" description="use magnetic compensation before takeoff only while GPS course not good"/>
     <define name="USE_AHRS_GPS_ACCELERATIONS" description="enable forward acceleration compensation from GPS speed"/>
+    <define name="ACCEL_WEIGHT_FILTER" value="8" description="adjust accel drift heuristic filter (default 8, 0 to disable filter)"/>
   </doc>
 
   <settings>

sw/airborne/subsystems/ahrs/ahrs_float_dcm.c

@@ -357,12 +357,18 @@ void Normalize(void)
   }
 }
 
+// strong structural vibrations can prevent to perform the drift correction
+// so accel magnitude is filtered before computing the weighting heuristic
+#ifndef ACCEL_WEIGHT_FILTER
+#define ACCEL_WEIGHT_FILTER 8
+#endif
 
 void Drift_correction()
 {
   //Compensation the Roll, Pitch and Yaw drift.
   static float Scaled_Omega_P[3];
   static float Scaled_Omega_I[3];
+  static float Accel_filtered = 0.f;
   float Accel_magnitude;
   float Accel_weight;
   float Integrator_magnitude;
@@ -377,9 +383,14 @@ void Drift_correction()
   // Calculate the magnitude of the accelerometer vector
   Accel_magnitude = sqrtf(accel_float.x * accel_float.x + accel_float.y * accel_float.y + accel_float.z * accel_float.z);
   Accel_magnitude = Accel_magnitude / GRAVITY; // Scale to gravity.
+#if ACCEL_WEIGHT_FILTER
+  Accel_filtered = (Accel_magnitude + (ACCEL_WEIGHT_FILTER - 1) * Accel_filtered) / ACCEL_WEIGHT_FILTER;
+#else // set ACCEL_WEIGHT_FILTER to 0 to disable filter
+  Accel_filtered = Accel_magnitude;
+#endif
   // Dynamic weighting of accelerometer info (reliability filter)
   // Weight for accelerometer info (<0.5G = 0.0, 1G = 1.0 , >1.5G = 0.0)
-  Accel_weight = Clip(1 - 2 * fabsf(1 - Accel_magnitude), 0, 1); //
+  Accel_weight = Clip(1.f - 2.f * fabsf(1.f - Accel_filtered), 0.f, 1.f);
 
 
 #if PERFORMANCE_REPORTING == 1