added a filter to the raw data before downsampling. This improves the…

… data quality dramatically. The mag is still getting eroneous readings, I think it has something to do with the FIFO reset.

src/drivers/imu/mpu9250/mpu9250.cpp

@@ -373,6 +373,10 @@ void MPU9250::run()
 		int16_t int_accel_y[42];
 		int16_t int_accel_z[42];
 
+		int16_t int_gyro_x[42];
+		int16_t int_gyro_y[42];
+		int16_t int_gyro_z[42];
+
 		float accel_x = 0.0f;
 		float accel_y = 0.0f;
 		float accel_z = 0.0f;
@@ -388,14 +392,17 @@ void MPU9250::run()
 			int_accel_y[i] = int16_t(_dma_data_buffer[_offset + 2 + i * 12] << 8 | _dma_data_buffer[_offset + 3 + i * 12]);
 			int_accel_z[i] = int16_t(_dma_data_buffer[_offset + 4 + i * 12] << 8 | _dma_data_buffer[_offset + 5 + i * 12]);
 
-			accel_x += int_accel_x[i];
-			accel_y += int_accel_y[i];
-			accel_z += int_accel_z[i];
+			int_gyro_x[i] = int16_t(_dma_data_buffer[_offset + 6 + i * 12] << 8 | _dma_data_buffer[_offset + 7 + i * 12]);
+			int_gyro_y[i] = int16_t(_dma_data_buffer[_offset + 8 + i * 12] << 8 | _dma_data_buffer[_offset + 9 + i * 12]);
+			int_gyro_z[i] = int16_t(_dma_data_buffer[_offset + 10 + i * 12] << 8 | _dma_data_buffer[_offset + 11 + i * 12]);
 
-			gyro_x 	+= int16_t(_dma_data_buffer[_offset + 6 + i * 12] << 8 | _dma_data_buffer[_offset + 7 + i * 12]);
-			gyro_y 	+= int16_t(_dma_data_buffer[_offset + 8 + i * 12] << 8 | _dma_data_buffer[_offset + 9 + i * 12]);
-			gyro_z 	+= int16_t(_dma_data_buffer[_offset + 10 + i * 12] << 8 | _dma_data_buffer[_offset + 11 + i * 12]);
+			accel_x += _accel_prefilter_x.apply(int_accel_x[i]);
+			accel_y += _accel_prefilter_y.apply(int_accel_y[i]);
+			accel_z += _accel_prefilter_z.apply(int_accel_z[i]);
 
+			gyro_x += _gyro_prefilter_x.apply(int_gyro_x[i]);
+			gyro_y += _gyro_prefilter_y.apply(int_gyro_y[i]);
+			gyro_z += _gyro_prefilter_z.apply(int_gyro_z[i]);
 
 			// This is our poor mans CRC. We look for duplicate accel data since the accel only produces data at 4KHz max.
 			bool accel_x_matched = int_accel_x[i] == (int16_t)(_dma_data_buffer[_offset + 0 + (i + 1) * 12] << 8 |
@@ -523,4 +530,4 @@ at 1KHz, the data is averaged and then published.
 int mpu9250_main(int argc, char *argv[])
 {
 	return MPU9250::main(argc, argv);
-}
+}

src/drivers/imu/mpu9250/mpu9250.h

@@ -38,6 +38,8 @@
 #include <lib/drivers/gyroscope/Gyro.hpp>
 #include <lib/drivers/magnetometer/Mag.hpp>
 
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+
 #include <ecl/geo/geo.h>
 
 #include <perf/perf_counter.h>
@@ -135,7 +137,7 @@
 #define ACCEL_SCALE ACCEL_FS_RANGE_M_S2 / 65535
 
 #define GYRO_SAMPLE_RATE	1000
-#define GYRO_FILTER_FREQ 80
+#define GYRO_FILTER_FREQ 30
 #define GYRO_FS_RANGE_RADS	(4000.0f * M_PI_F) / 180
 #define GYRO_SCALE GYRO_FS_RANGE_RADS / 65535
 
@@ -295,6 +297,15 @@ class MPU9250 : public ModuleBaseMulti<MPU9250>
 	perf_counter_t		_fifo_maxed;
 	perf_counter_t		_reset;
 
+	math::LowPassFilter2p _accel_prefilter_x{4000, 100};
+	math::LowPassFilter2p _accel_prefilter_y{4000, 100};
+	math::LowPassFilter2p _accel_prefilter_z{4000, 100};
+
+	math::LowPassFilter2p _gyro_prefilter_x{32000, 100};
+	math::LowPassFilter2p _gyro_prefilter_y{32000, 100};
+	math::LowPassFilter2p _gyro_prefilter_z{32000, 100};
+
+
 	unsigned _offset = 1;
 
 	bool _just_reset = false;
@@ -326,4 +337,4 @@ class MPU9250 : public ModuleBaseMulti<MPU9250>
 	 * @return 		PX4_OK on success, PX4_ERROR on failure
 	 */
 	int 			self_test();
-};
+};