MPU6050传感器数据滤波

奚海蛟

已于 2022-03-28 09:19:15 修改

阅读量4.5k

点赞数 4

分类专栏：嵌入式应用开发-无人机飞控文章标签： stm32 arm 物联网 c语言单片机

于 2022-03-28 09:17:11 首次发布

本文链接：https://blog.csdn.net/xhj1021/article/details/123786440

版权

嵌入式应用开发-无人机飞控专栏收录该内容

10 篇文章 10 订阅

订阅专栏

目的

练习对加速度传感器的原始数据进行滤波算法的实验，学习使用平滑滤波、FIR、IIR等的滤波算法。

原理

滤波器：选择所需的某一或某些频带的信号而抑制不需要的其它频带的信号。

通带：滤波器中能使信号通过的频带，通带边缘所对应的频率称为通带截止频率。

阻带：抑制噪声通过的频带。

过渡带：从通带到阻带的过渡频率范围。

滤波器分类

输入输出信号：模拟和数字滤波器；

DF按照单位取样响应或实现网络结构又分为：IIR DF和FIR DF，又称递归型和非递归型；

低通滤波器：只允许低频信号通过而抑制高频信号。例如，可用低通滤波器消除旧音乐录音带中的背景噪声。

高通滤波器：只允许高频信号通过而抑制低频信号。例如，声纳系统可用高通滤波器消除信号中的船和海浪的低频噪声，保留目标特征。

带通滤波器：允许某一频带的信号通过。例如，数字电话双音多频（DTMF）信号的解码，每个电话键产生一对音频信号，其中一个信号对按键的行编码，另一个对列编码，接收端通过一组带通滤波器来识别每个按键。

带阻滤波器：抑制某一频带的信号。例如，从复合电视信号中滤除频分复用的色度信号，以便得到亮度信号。

本实验对加速度传感器的原始数据进行平滑滤波，旨在学会使用平滑滤波。如图1.1为本次实验滑动滤波流程，对当前20组数据求和取平均值作为当前传感器测得的数据，程序实现见代码。

步骤

以上一个MPU6050传感器驱动实验为基础，打开MPU6050传感器驱动工程。
在bsp_mpu6050.c文件中添加MPU6050_newValue（）滑动滤波函数，通过调用此函数进行传感器数据滑动滤波处理。

float AVG_Filter[ITEMS] [(FILTER_NUM + 1)];
//平滑滤波
void MPU6050_SMO_Filter()
{
	uint8_t i;
	float FILT_TMP[ITEMS] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f};
	static uint8_t flt_idx = 0;

	if( ++flt_idx > FILTER_NUM )	
	{
		flt_idx = 0;
	}
	//更新滑动滤波窗口数据
	AVG_Filter[A_X][flt_idx] =  (mpu6050.acc_raw.x - mpu6050.acc_offset.x);//填值
	AVG_Filter[A_Y][flt_idx] =  (mpu6050.acc_raw.y - mpu6050.acc_offset.y);
	AVG_Filter[A_Z][flt_idx] =  (mpu6050.acc_raw.z - mpu6050.acc_offset.z);
	AVG_Filter[G_X][flt_idx] =  (mpu6050.gyro_raw.x - mpu6050.gyro_offset.x);
	AVG_Filter[G_Y][flt_idx] =  (mpu6050.gyro_raw.y - mpu6050.gyro_offset.y);
	AVG_Filter[G_Z][flt_idx] =  (mpu6050.gyro_raw.z - mpu6050.gyro_offset.z);

	for(i=0;i<FILTER_NUM;i++)
	{
		FILT_TMP[A_X] += AVG_Filter[A_X][i];//累加
		FILT_TMP[A_Y] += AVG_Filter[A_Y][i];
		FILT_TMP[A_Z] += AVG_Filter[A_Z][i];
		FILT_TMP[G_X] += AVG_Filter[G_X][i];
		FILT_TMP[G_Y] += AVG_Filter[G_Y][i];
		FILT_TMP[G_Z] += AVG_Filter[G_Z][i];
	}

	mpu6050.acc.x = (float)( FILT_TMP[A_X] )/(float)FILTER_NUM ;//求均值
	mpu6050.acc.y = (float)( FILT_TMP[A_Y] )/(float)FILTER_NUM ;
	mpu6050.acc.z = (float)( FILT_TMP[A_Z] )/(float)FILTER_NUM ;
		
	mpu6050.gyro.x = ((float)( FILT_TMP[G_X] )/(float)FILTER_NUM )*GYRO_SCALE;
	mpu6050.gyro.y = ((float)( FILT_TMP[G_Y] )/(float)FILTER_NUM )*GYRO_SCALE;
	mpu6050.gyro.z = ((float)( FILT_TMP[G_Z] )/(float)FILTER_NUM )*GYRO_SCALE;
}

在bsp_mpu6050.c文件中，修改mems_data()函数，加入滑动滤波函数。具体修改后如下所示：

//采集原始值，并进行滤波
void mems_data(void)
{
	MPU6050_Read(mpu6050_buffer); 	//读取mpu6轴传感器
	
	/*解析原始数据，并且进行坐标转换*/
	mpu6050.acc_raw.x	=(int16_t)(mpu6050_buffer[0] << 8 | mpu6050_buffer[1]);
	mpu6050.acc_raw.y	=(int16_t)(mpu6050_buffer[2] << 8 | mpu6050_buffer[3]); 	
	mpu6050.acc_raw.z =	(int16_t)(mpu6050_buffer[4] << 8 | mpu6050_buffer[5]);	
	
	//陀螺仪读取
	mpu6050.gyro_raw.x  =(int16_t)(mpu6050_buffer[8] << 8 | mpu6050_buffer[9]);
	mpu6050.gyro_raw.y	=(int16_t)(mpu6050_buffer[10] << 8 | mpu6050_buffer[11]);
	mpu6050.gyro_raw.z	=(int16_t)(mpu6050_buffer[12] << 8 | mpu6050_buffer[13]);
		//数据滤波
	MPU6050_SMO_Filter();
}

编译并下载，接下来将使用“STM-studio”进行数据的查看，安装并配置好环境。安装以后打开软件，找到“File”->“Import variables from executable”。STM-studio下载链接。