Fix MPU6000 spikes in certain chipbatches (#2759)

* Fix MPU6000 spikes in certain chipbatches

* Adjusted according to mergerequest codereview remarks

Co-authored-by: Open UAS <noreply@openuas.org>

conf/modules/imu_mpu6000.xml

@@ -3,7 +3,7 @@
 <module name="imu_mpu6000" dir="imu">
   <doc>
     <description>
-      IMU with MPU6000 via SPI.
+      IMU with an Invensense MPU6000 connected via the SPI bus to flightcontroller.
     </description>
     <configure name="IMU_MPU_SPI_DEV" value="spi1" description="SPI device to use for MPU6000"/>
     <configure name="IMU_MPU_SPI_SLAVE_IDX" value="SPI_SLAVE0" description="slave index of the MPU CS pin"/>
@@ -11,6 +11,7 @@
     <define name="IMU_MPU_SMPLRT_DIV" value="3" description="sample rate divider setting of the MPU"/>
     <define name="IMU_MPU_GYRO_RANGE" value="MPU60X0_GYRO_RANGE_2000" description="gyroscope range setting of the MPU"/>
     <define name="IMU_MPU_ACCEL_RANGE" value="MPU60X0_ACCEL_RANGE_16G" description="accelerometer range setting of the MPU"/>
+    <define name="IMU_MPU_USE_MEDIAN_FILTER" value="FALSE" description="Use ONLY in case the MPU has spikes in the raw output due to various reasons. Default is FALSE"/>
   </doc>
   <dep>
     <depends>spi_master</depends>

conf/radios/OPENUAS/openuas_rxsr_cppm_8ch.xml

@@ -0,0 +1,17 @@
+<!--
+The order of the list below is of importance if you do not define a
+"no=" (order in the PPM frame) parameter.
+If you do not define this then the order of the PPM is the one of
+the order of the functon in the list
+-->
+<!DOCTYPE radio SYSTEM "../radio.dtd">
+<radio name="OpenUAS_OpenRXSR" data_min="980" data_max="2100" sync_min="5000" sync_max="16000" pulse_type="NEGATIVE">
+  <channel function="THROTTLE" min="980" neutral="990" max="2040" average="0"/>
+  <channel function="ROLL" min="1000" neutral="1500" max="1900" average="0"/>
+  <channel function="PITCH" min="1000" neutral="1500" max="1900" average="0"/>
+  <channel function="YAW" min="1000" neutral="1500" max="1900" average="0"/>
+  <channel function="MODE" min="1000" neutral="1500" max="1900" average="2"/>
+  <channel function="AUX2" min="1000" neutral="1100" max="1900" average="4"/>
+  <channel function="AUX3" min="1000" neutral="1500" max="1900" average="6"/>
+  <channel function="AUX4" min="1000" neutral="1500" max="1900" average="6"/> 
+</radio>

conf/userconf/OPENUAS/openuas_all_ac.xml

@@ -1,4 +1,16 @@
 <conf>
+  <aircraft
+   name="A160"
+   ac_id="222"
+   airframe="airframes/OPENUAS/openuas_wltech_xk_a160.xml"
+   radio="radios/OPENUAS/openuas_rxsr_cppm_8ch.xml"
+   telemetry="telemetry/OPENUAS/openuas_fixedwing_imu_rc.xml"
+   flight_plan="flight_plans/basic.xml"
+   settings="settings/fixedwing_basic.xml settings/control/ctl_new.xml"
+   settings_modules="modules/ahrs_float_cmpl_quat.xml modules/geo_mag.xml modules/gps.xml modules/guidance_full_pid_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/photogrammetry_calculator.xml modules/stabilization_adaptive_fw.xml"
+   gui_color="#ffffd7b80000"
+   release="4446ea27fbf9191469fb70885257f345a57d4483"
+  />
   <aircraft
    name="ARDrone_2"
    ac_id="238"

sw/airborne/subsystems/imu/imu_mpu6000.c

@@ -28,12 +28,14 @@
 #include "subsystems/imu.h"
 #include "subsystems/abi.h"
 #include "mcu_periph/spi.h"
+#if IMU_MPU_USE_MEDIAN_FILTER
+#include "filters/median_filter.h"
+#endif
 
 /* SPI defaults set in subsystem makefile, can be configured from airframe file */
 PRINT_CONFIG_VAR(IMU_MPU_SPI_SLAVE_IDX)
 PRINT_CONFIG_VAR(IMU_MPU_SPI_DEV)
 
-
 /* MPU60x0 gyro/accel internal lowpass frequency */
 #if !defined IMU_MPU_LOWPASS_FILTER && !defined  IMU_MPU_SMPLRT_DIV
 #if (PERIODIC_FREQUENCY >= 60) && (PERIODIC_FREQUENCY <= 120)
@@ -102,11 +104,21 @@ PRINT_CONFIG_VAR(IMU_MPU_Y_SIGN)
 #endif
 PRINT_CONFIG_VAR(IMU_MPU_Z_SIGN)
 
-
 struct ImuMpu6000 imu_mpu_spi;
 
+#if IMU_MPU_USE_MEDIAN_FILTER
+static struct MedianFilter3Int medianfilter_accel;
+static struct MedianFilter3Int medianfilter_rates;
+#endif
+
 void imu_mpu_spi_init(void)
 {
+
+#if IMU_MPU_USE_MEDIAN_FILTER
+  InitMedianFilterVect3Int(medianfilter_accel, 3);
+  InitMedianFilterRatesInt(medianfilter_rates, 3);
+#endif
+
   mpu60x0_spi_init(&imu_mpu_spi.mpu, &IMU_MPU_SPI_DEV, IMU_MPU_SPI_SLAVE_IDX);
   // change the default configuration
   imu_mpu_spi.mpu.config.smplrt_div = IMU_MPU_SMPLRT_DIV;
@@ -116,7 +128,6 @@ void imu_mpu_spi_init(void)
   imu_mpu_spi.mpu.config.accel_range = IMU_MPU_ACCEL_RANGE;
 }
 
-
 void imu_mpu_spi_periodic(void)
 {
   mpu60x0_spi_periodic(&imu_mpu_spi.mpu);
@@ -139,6 +150,12 @@ void imu_mpu_spi_event(void)
       IMU_MPU_Y_SIGN * (int32_t)(imu_mpu_spi.mpu.data_rates.value[IMU_MPU_CHAN_Y]),
       IMU_MPU_Z_SIGN * (int32_t)(imu_mpu_spi.mpu.data_rates.value[IMU_MPU_CHAN_Z])
     };
+
+    // In case sensor exhibits faulty large spike values in raw output remove them
+#if IMU_MPU_USE_MEDIAN_FILTER
+    UpdateMedianFilterVect3Int(medianfilter_accel, accel);
+    UpdateMedianFilterRatesInt(medianfilter_rates, rates);
+#endif
     // unscaled vector
     VECT3_COPY(imu.accel_unscaled, accel);
     RATES_COPY(imu.gyro_unscaled, rates);