STM32 使用DMP库处理MPU6050数据

0、系列目录

1. STM32 软件模拟IIC
2. STM32 使用DMP库处理MPU6050数据
3. STM32 MPU6050与匿名上位机通讯（V2.6版）

1、实验分析

MPU6050包含一个三轴陀螺仪，三轴加速度计，并且可以通过AUX_CL和AUX_DA再扩展一个磁力计，内部设有一个可扩展的数字运动处理器DMP，可以将欧拉角以四元数的形式输出。本次实验我们使用DMP库对MPU6050初始化并且通过DMP中的库函数读取加速度值和陀螺仪值显示在LCD上

2、实验前准备

使用DMP库需要在MPU官方下载固件并将下面六个导入工程

并且在固件库中需要进行一定的设定。
在inv_mpu.c文件中预留有调用文件的接口，需要我们提前设定一下
首先

#define MPU6050 //定义我们使用的传感器为MPU6050
#define MOTION_DRIVER_TARGET_MSP430 //定义驱动部分,采用MSP430的驱动(移植到STM32F1)

其次
要对接接口函数

/* The following functions must be defined for this platform:
 1. i2c_write(unsigned char slave_addr, unsigned char reg_addr,
 2.      unsigned char length, unsigned char const *data)
 3. i2c_read(unsigned char slave_addr, unsigned char reg_addr,
 4.      unsigned char length, unsigned char *data)
 5. delay_ms(unsigned long num_ms)
 6. get_ms(unsigned long *count)
 7. reg_int_cb(void (*cb)(void), unsigned char port, unsigned char pin)
 8. labs(long x)
 9. fabsf(float x)
 10. min(int a, int b)
 */
#define i2c_write   MPU_Write_Len
#define i2c_read    MPU_Read_Len
#define delay_ms    delay_ms
#define get_ms      mget_ms
#define log_i 	printf	//打印信息
#define log_e  	printf	//打印信息

按照官方注释的要求，接口函数的入口参数必须按照要求对齐，否则将无法使用DMP库函数。

3、实验分析

上一次我们编写了软模拟IIC的程序，本次需要增加IIC连续写和连续读，这样我们才可以直接操作MPU中的寄存器。
操作步骤：
IIC初始化-》MPU的初始化-》DMP库初始化-》使用库函数读取数据
（1）MPU初试步骤

1. 设置AD0引脚电平（低电平，从机地址为0x68；高电平，从机地址为0x69）
2. IIC初始化
3. 复位从机，唤醒从机
4. 设置陀螺仪、加速度计、设置采样速率
5. 关闭所有中断、IIC主模式关闭、关闭FIFO
6. 设置INT引脚低电平有效
7. 判断从机地址是否正确
8. 设置正方向
9. 使能陀螺仪和加速度计

4、MPU6050寄存器分析

1、自检寄存器

MPU自检寄存器，会产生自检反馈：STR=Gyroscope Output with Self Test Enabled — Gyroscope Output with Self Test Disabled。可以判断自检成功or失败
2、采样频率分频器

传感器寄存器输出， FITO 输出， DMP 采样， Motion 检测， Zero Motion 检测和 Free Fall检测都基于这个采样频率。
采样频率=陀螺仪输出频率/（ 1+SMPLRT_DIV）
陀螺仪的输出频率是1KHz或者8KHz，与数字低通滤波器（DLPF）有关，当DLPF_CFG=0/7时，频率为8K，其他为1K，而且 DLPF 滤波频率一般设置为采样率的一半。采样率，我们假定设置为 50Hz，那么SMPLRT_DIV=1000/50-1=19。
3、陀螺仪配置寄存器

这个寄存器我们只需要设置FS_SEL位，用于设置陀螺仪满量程范围：0，±250°/s；1，±500°/s；2，±1000°/s；3，±2000°/s
陀螺仪为16位的ADC，所以灵敏度可以达到65536/4000=16.4LSB/(° /S)。
4、加速度计配置寄存器

和陀螺仪一样，只需要设置AFS_SEL位，0，±2g； 1，±4g； 2，±8g； 3，±16g；我们一般设置为 0，即±2g，因为加速度传感器的 ADC,也是 16 位，所以得到灵敏度为： 65536/4=16384LSB/g。
5、FIFO使能寄存器

该寄存器用于控制 FIFO 使能，在简单读取传感器数据的时候，可以不用 FIFO，设置对应位为 0 即可禁止 FIFO，设置为 1，则使能 FIFO。注意加速度传感器的 3 个轴，全由 1 个位（ACCEL_FIFO_EN）控制，只要该位置 1，则加速度传感器的三个通道都开启 FIFO。
6、配置寄存器

这里的DLPF_CFG位就是数字低通滤波器的设置位

一般我们设置角速度传感器的带宽为其采样率的一半，比如前面设置的采样率为100Hz，那么带宽就应该设置50Hz，取表中角速度近似值42Hz，就应该设置DLPF_CFG=011;
7、电源管理寄存器1

DEVICE_RESET 位用来控制复位，设置为 1，复位 MPU6050，复位结束后， MPU硬件自动清零该位。
SLEEEP 位用于控制 MPU6050 的工作模式，复位后，该位为 1，即进入了睡眠模式（低功耗），所以我们要清零该位，以进入正常工作模式。
TEMP_DIS 用于设置是否使能温度传感器，设置为 0，则使能。
CLKSEL[2:0]用于选择系统时钟源

内部8M晶振，不太准确，一般采用X轴的PLL作为时钟源
8、电源管理寄存器2

LP_WAKE_CTRL 用于控制低功耗时的唤醒频率，用不到。
剩下的 6 位，分别控制加速度和陀螺仪的 x/y/z 轴是否进入待机模式，这里我们全部都不进入待机模式，所以全部设置为 0 即可。
9、数据输出寄存器
陀螺仪数据输出寄存器，总共有 6 个寄存器组成，
地址为： 0X43~0X48，
通过读取这 6 个寄存器，就可以读到陀螺仪 x/y/z 轴的值，
比如 x 轴的数据，可以通过读取 0X43（高 8 位）和 0X44（低 8 位）寄存器得到，其他轴以此类推。
同样，加速度传感器数据输出寄存器，也有 6 个，地址为： 0X3B~0X40，
通过读取这 6 个寄存器，就可以读到加速度传感器 x/y/z 轴的值，
比如读 x 轴的数据，可以通过读取 0X3B（高8 位）和 0X3C（低 8 位）寄存器得到，其他轴以此类推。
10、温度传感器
温度传感器的值，可以通过读取 0X41（高 8 位）和 0X42（低 8 位）寄存器得到，温度换算公式为：
Temperature = 36.53 + regval/340
Temperature 为计算得到的温度值，单位为℃， regval 为从 0X41 和 0X42 读到的温度传感器值。

5、代码示例

1、读写数据

/**  
  *  功能：IIC写一个字节
  *  入口参数：reg，寄存器地址；data，数据
  *  返回值：0，正常；1，错误
  */
u8 MPU_Write_Byte(u8 reg,u8 data)
{
    
    
	MPU_IIC_Start();
	MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//ADDR左移一位+写操作
	if(MPU_IIC_Wait_Ack())//等待应答
	{
    
    
		MPU_IIC_Stop();
		return 1;
	}
	MPU_IIC_Send_Byte(reg);
	MPU_IIC_Wait_Ack();
	MPU_IIC_Send_Byte(data);
	if(MPU_IIC_Wait_Ack())
	{
    
    
		MPU_IIC_Stop();
		return 1;
	}
	MPU_IIC_Stop();
	return 0;
}
  
/**  
  *  功能：IIC读一个字节
  *  入口参数：reg，寄存器地址
  *  返回值：数据
  */
u8 MPU_Read_Byte(u8 reg)
{
    
    
	u8 res;
	MPU_IIC_Start();
	MPU_IIC_Send_Byte((MPU_ADDR<<1)|0);//外设地址+写命令
	MPU_IIC_Wait_Ack();
	MPU_IIC_Send_Byte(reg);//寄存器地址
	MPU_IIC_Wait_Ack();
	MPU_IIC_Start();
	MPU_IIC_Send_Byte((MPU_ADDR<<1)|1);
	MPU_IIC_Wait_Ack();
	res = MPU_IIC_Read_Byte(0);
	MPU_IIC_Stop();
	return res;
}
/**  
  *  功能：IIC连续写
  *  入口参数：addr，外设地址；reg，寄存器地址；len，写入长度；buf，数据
  *  返回值：0，正常；1，错误
  */
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    
    
	u8 i;
	MPU_IIC_Start();
	MPU_IIC_Send_Byte((addr<<1)|0);
	if(MPU_IIC_Wait_Ack())
	{
    
    
		MPU_IIC_Stop();
		return 1;
	}
	MPU_IIC_Send_Byte(reg);
	MPU_IIC_Wait_Ack();
	for(i=0;i<len;i++)
	{
    
    
		MPU_IIC_Send_Byte(buf[i]);
		if(MPU_IIC_Wait_Ack())
		{
    
    
			MPU_IIC_Stop();
			return 1;
		}
	}
	MPU_IIC_Stop();
	return 0;
}
/**  
  *  功能：IIC连续读
  *  入口参数：addr，外设地址；reg，寄存器地址；len，读取长度；buf，储存数据
  *  返回值：0,正常；1，错误
  */
u8 MPU_Read_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
    
    
	MPU_IIC_Start();
	MPU_IIC_Send_Byte((addr<<1)|0);
	if(MPU_IIC_Wait_Ack())
	{
    
    
		MPU_IIC_Stop();
		return 1;
	}
	MPU_IIC_Send_Byte(reg);
	MPU_IIC_Wait_Ack();
	MPU_IIC_Start();
	MPU_IIC_Send_Byte((addr<<1)|1);
	MPU_IIC_Wait_Ack();
	while(len)
	{
    
    
		if(len==1)
			*buf=MPU_IIC_Read_Byte(0);
		else		
			*buf=MPU_IIC_Read_Byte(1);		
		len--;
		buf++;
	}
	MPU_IIC_Stop();
	return 0;
}

2、陀螺仪初始化代码

/**  
  *  功能：初始化MPU6050
  *  入口参数：无
  *  返回值：0，成功；其他，错误
  */
u8 MPU_Init(void)
{
    
    
	u8 res;
	GPIO_InitTypeDef GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO | RCC_APB2Periph_GPIOA,ENABLE);
	
	GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;
	GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);//禁止JTAG，使PA15变为普通IO
	
	MPU_AD0_CTRL = 0;//控制MPU6050的AD0为低电平，从机地址=0x68；
					 //					 高电平，		  0x69；
	MPU_IIC_Init();
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x80);//复位从机
	delay_ms(100);
	MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x00);//唤醒从机
	MPU_Set_Gyro_Fsr(3);//陀螺仪传感器 +-2000dps
	MPU_Set_Accel_Fsr(0);//加速度传感器 +-2g
	MPU_Set_Rate(50);
	MPU_Write_Byte(MPU_INT_EN_REG,0x00);//关闭所有中断
	MPU_Write_Byte(MPU_USER_CTRL_REG,0x00);//IIC主模式关闭
	MPU_Write_Byte(MPU_FIFO_EN_REG,0x00);//关闭FIFO
	MPU_Write_Byte(MPU_INTBP_CFG_REG,0x80);//INT低电平有效
	res = MPU_Read_Byte(MPU_DEVICE_ID_REG);
	if(res == MPU_ADDR)
	{
    
    
		MPU_Write_Byte(MPU_PWR_MGMT1_REG,0x01);//设置CLKSEL PLL X轴为参考
		MPU_Write_Byte(MPU_PWR_MGMT2_REG,0x00);//加速度计和陀螺仪使能
		MPU_Set_Rate(50);
	}
	else
		return 1;
	return 0;
}
/**  
  *  功能：设置陀螺仪传感器满量程范围
  *  入口参数：fsr,0,+-250dps
  *				   1,+-500dps
  *				   2,+-1000dps
  *				   3,+-2000dps	
  *  返回值：0,成功；1，失败
  */
u8 MPU_Set_Gyro_Fsr(u8 fsr)
{
    
    
	return MPU_Write_Byte(MPU_GYRO_CFG_REG,fsr<<3);
}
/**  
  *  功能：设置加速度传感器满量程范围
  *  入口参数：fsr，0,+-2g
  *				    1,+-4g
  *				    2,+-8g
  *				    3,+-16g	
  *  返回值：0,成功；1，失败
  */
u8 MPU_Set_Accel_Fsr(u8 fsr)
{
    
    
	return MPU_Write_Byte(MPU_ACCEL_CFG_REG,fsr<<3);
}
/**  
  *  功能：设置数字低通滤波器
  *  入口参数：lpf，低通滤波器频率（Hz）
  *  返回值：0,成功；1，失败
  */
u8 MPU_Set_LPF(u16 lpf)
{
    
    
	u8 data=0;
	if(lpf>=188)data=1;
	else if(lpf>=98)data=2;
	else if(lpf>=42)data=3;
	else if(lpf>=20)data=4;
	else if(lpf>=10)data=5;
	else data=6;
	return MPU_Write_Byte(MPU_CFG_REG,data);
}
/**  
  *  功能：设置采样率（Fs=1KHz）
  *  入口参数：rate=[4,1000]Hz
  *  返回值：0,设置成功；1，设置失败
  */
u8 MPU_Set_Rate(u16 rate)
{
    
    
	u8 data;
	if(rate>1000)rate=1000;
	if(rate<4)rate=4;
	data=1000/rate-1;
	data=MPU_Write_Byte(MPU_SAMPLE_RATE_REG,data);
	return MPU_Set_LPF(rate/2);
}

3、获取原始数据代码

/**  
  *  功能：获取MPU内部温度
  *  入口参数：无
  *  返回值：温度*100
  */
short MPU_Get_Temperature(void)
{
    
    
	u8 buf[2];
	short raw;
	float temp;
	MPU_Read_Len(MPU_ADDR,MPU_TEMP_OUTH_REG,2,buf);
	raw=((u16)buf[0]<<8)|buf[1];
	temp = 36.53+((double)raw)/340;
	return temp*100;
}
/**  
  *  功能：获得陀螺仪原始值
  *  入口参数：gx,gy,gz,陀螺仪XYZ轴原始数据（带符号）
  *  返回值：0，成功；1，失败
  */
u8 MPU_Get_Gyroscope(short *gx,short *gy,short *gz)
{
    
    
	u8 buf[6],res;
	res=MPU_Read_Len(MPU_ADDR,MPU_GYRO_XOUTH_REG,6,buf);
	if(res == 0)
	{
    
    
		*gx=((u16)buf[0]<<8)|buf[1];
		*gy=((u16)buf[2]<<8)|buf[3];
		*gz=((u16)buf[4]<<8)|buf[5];
	}
	return res;
}
/**  
  *  功能：获得加速度原始值
  *  入口参数：ax,ay,az,加速度计XYZ的原始数据（）
  *  返回值：0，正常；1，错误
  */
u8 MPU_Get_Accelerometer(short *ax,short *ay,short *az)
{
    
    
	u8 buf[6],res;
	res=MPU_Read_Len(MPU_ADDR,MPU_ACCEL_XOUTH_REG,6,buf);
	if(res ==0)
	{
    
    
		*ax=((u16)buf[0]<<8)|buf[1];
		*ay=((u16)buf[2]<<8)|buf[3];
		*az=((u16)buf[4]<<8)|buf[5];
	}
	return res;
}

4、DMP库函数调用

扫描二维码关注公众号，回复： 13568882 查看本文章

//mpu6050,dmp初始化
//返回值:0,正常
//    其他,失败
u8 mpu_dmp_init(void)
{
    
    
	u8 res=0;
	MPU_IIC_Init(); 	//初始化IIC总线
	if(mpu_init()==0)	//初始化MPU6050
	{
    
    	 
		res=mpu_set_sensors(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置所需要的传感器
		if(res)return 1; 
		res=mpu_configure_fifo(INV_XYZ_GYRO|INV_XYZ_ACCEL);//设置FIFO
		if(res)return 2; 
		res=mpu_set_sample_rate(DEFAULT_MPU_HZ);	//设置采样率
		if(res)return 3; 
		res=dmp_load_motion_driver_firmware();		//加载dmp固件
		if(res)return 4; 
		res=dmp_set_orientation(inv_orientation_matrix_to_scalar(gyro_orientation));//设置陀螺仪方向
		if(res)return 5; 
		res=dmp_enable_feature(DMP_FEATURE_6X_LP_QUAT|DMP_FEATURE_TAP|	//设置dmp功能
		    DMP_FEATURE_ANDROID_ORIENT|DMP_FEATURE_SEND_RAW_ACCEL|DMP_FEATURE_SEND_CAL_GYRO|
		    DMP_FEATURE_GYRO_CAL);
		if(res)return 6; 
		res=dmp_set_fifo_rate(DEFAULT_MPU_HZ);	//设置DMP输出速率(最大不超过200Hz)
		if(res)return 7;   
		res=run_self_test();		//自检
		if(res)return 8;    
		res=mpu_set_dmp_state(1);	//使能DMP
		if(res)return 9;     
	}else return 10;
	return 0;
}
//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
//返回值:0,正常
//    其他,失败
u8 mpu_dmp_get_data(float *pitch,float *roll,float *yaw)
{
    
    
	float q0=1.0f,q1=0.0f,q2=0.0f,q3=0.0f;
	unsigned long sensor_timestamp;
	short gyro[3], accel[3], sensors;
	unsigned char more;
	long quat[4]; 
	if(dmp_read_fifo(gyro, accel, quat, &sensor_timestamp, &sensors,&more))return 1;	 
	/* Gyro and accel data are written to the FIFO by the DMP in chip frame and hardware units.
	 * This behavior is convenient because it keeps the gyro and accel outputs of dmp_read_fifo and mpu_read_fifo consistent.
	**/
	/*if (sensors & INV_XYZ_GYRO )
	send_packet(PACKET_TYPE_GYRO, gyro);
	if (sensors & INV_XYZ_ACCEL)
	send_packet(PACKET_TYPE_ACCEL, accel); */
	/* Unlike gyro and accel, quaternions are written to the FIFO in the body frame, q30.
	 * The orientation is set by the scalar passed to dmp_set_orientation during initialization. 
	**/
	if(sensors&INV_WXYZ_QUAT) 
	{
    
    
		q0 = quat[0] / q30;	//q30格式转换为浮点数
		q1 = quat[1] / q30;
		q2 = quat[2] / q30;
		q3 = quat[3] / q30; 
		//计算得到俯仰角/横滚角/航向角
		*pitch = asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;	// pitch
		*roll  = atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;	// roll
		*yaw   = atan2(2*(q1*q2 + q0*q3),q0*q0+q1*q1-q2*q2-q3*q3) * 57.3;	//yaw
	}else return 2;
	return 0;
}

6、实验现象

7、总结

对于MPU6050数据的处理，使用DMP库是一种比较简单的办法，如果理论扎实，可以自行读取原始数据，通过四元数解算获取欧拉角。下一次我们将会把陀螺仪得到的数据发送到上位机，下次主要分析上位机的通讯协议。