MPU9250将原始数据平均值滤波后再用自带DMP/MPL输出欧拉角，效果挺好！

MPU9250/MPU6050

自带DMP输出的效果不是很理想，在原始数上滤波后再用自带DMP输出数据很平稳

多的不说，直接附上部分代码

链接: link.

在inv_mpu.c中更改如下代码： 只拿角速度原数据举例

int mpu_get_gyro_reg(short *data, unsigned long *timestamp)
{
    unsigned char tmp[6];
	  short gyro[3];

    if (!(st.chip_cfg.sensors & INV_XYZ_GYRO))
        return -1;

    if (i2c_read(st.hw->addr, st.reg->raw_gyro, 6, tmp))
        return -1;
    gyro[0] = (tmp[0] << 8) | tmp[1];
    gyro[1] = (tmp[2] << 8) | tmp[3];
    gyro[2] = (tmp[4] << 8) | tmp[5];
		    if (timestamp)
        get_ms(timestamp);

	static float buf_X[W],buf_Y[W],buf_Z[W];  
	static uint8_t cnt =0,flag = 1;
	float temp1=0,temp2=0,temp3=0;
	uint8_t n=12;  
	
	uint8_t i;
	buf_X[cnt] = gyro[0];  
	buf_Y[cnt] = gyro[1];
	buf_Z[cnt] = gyro[2];
	
	cnt++;     
	if(cnt<n && flag) 

		return 0;   

	else
		flag = 0;  
	
	QuiteSort(buf_X,0,n-1);
	QuiteSort(buf_Y,0,n-1);
	QuiteSort(buf_Z,0,n-1);
	for(i=1;i<n-1;i++)
	 {
		temp1 += buf_X[i];  
		temp2 += buf_Y[i];
		temp3 += buf_Z[i];
	 }

	 if(cnt>=n) 
	 {cnt = 0;  
	 data[0] = temp1/(n-2);
	 data[1] = temp2/(n-2);
	 data[2] = temp3/(n-2);	
	 }			
//		printf("%d %d %d\n",data[0],data[1],data[2]);
    return 0;
}
```javascript