利用TM4C123G单片机读取并处理MPU6050初始数据

一、MPU6050简介

网络上关于这款芯片的介绍很多，这里简单概括一下

MPU6050简介

MPU6050 内部有 3 轴陀螺仪和 3 轴加速度传感器，其中陀螺仪可以测得芯片当前的角速度，而加速度传感器可以测得芯片当前感应到的加速度。通过读取并处理这两个传感器的数据，我们可以分别求得两个独立的传感器姿态数据，即传感器坐标系相对于地面坐标系x、y、z轴的相对角度偏移，然后通过特定的算法整合这两组数据，从而求得精确的姿态数据，求得当前的欧拉角。

MPU6050结构框图

本程序需要使用的模块已被框出

二、关于该芯片的使用

芯片的使用方法可以总结为以下几个步骤
1、主程序必要模块初始化（IIC等）
2、利用IIC写入芯片寄存器，初始化芯片
3、利用IIC读取芯片数据
4、数据处理
注：在本程序中，MPU6050的IIC地址是0x68
芯片的接线：

MPU6050接口	单片机接口
VDD	3.3V
GND	任意GND接口
SCL	PA6
SDA	PA7
INT	N/A
AD0	N/A
芯片的初始化：

void MPU6050_Init(void)//MPU6050初始化
{
	IMU_ID=Single_ReadI2C(MPU_ADDR,WHO_AM_I);
	printf("%d\n",IMU_ID);//检验IIC是否能对MPU6050进行操作
  i2cWrite(MPU_ADDR,MPU_PWR_MGMT1_REG,0X80);//对MPU6050寄存器清零
	delay_ms(100);
	i2cWrite(MPU_ADDR,MPU_PWR_MGMT1_REG,0X00);//唤醒MPU6050
	i2cWrite(MPU_ADDR,MPU_GYRO_CFG_REG,0X00);//设置陀螺仪满量程范围为250°
	i2cWrite(MPU_ADDR,MPU_ACCEL_CFG_REG,0X00);//设置加速度传感器满量程范围为±2G
	i2cWrite(MPU_ADDR,MPU_SAMPLE_RATE_REG,0X13);//设置采样率
	i2cWrite(MPU_ADDR,MPU_INT_EN_REG,0X00);//关闭所有中断（本程序不需要）
	i2cWrite(MPU_ADDR,MPU_USER_CTRL_REG,0X00);
	i2cWrite(MPU_ADDR,MPU_FIFO_EN_REG,0X00);//关闭FIFO
	i2cWrite(MPU_ADDR,MPU_INTBP_CFG_REG,0X80);
	i2cWrite(MPU_ADDR,MPU_PWR_MGMT1_REG,0X01);
	i2cWrite(MPU_ADDR,MPU_PWR_MGMT2_REG,0X00);
}

注意：在“对MPU6050清零操作”和“唤醒MPU6050操作”间，必须使用延时函数间隔（一般是100ms），否则初始化函数后续操作将无效

三、代码

1.MPU6050数据的读取

void MPU6050_Read_Data(vector3f *gyro,vector3f *accel,float *temperature)
{
	uint8_t buf[14];
	int16_t temp;
	i2cReadData(MPU_ADDR,ACCEL_XOUT_H,buf,14);
	accel->x=(int16_t)((buf[0]<<8)|buf[1]);
	accel->y=-(int16_t)((buf[2]<<8)|buf[3]);
	accel->z=-(int16_t)((buf[4]<<8)|buf[5]);
	temp=(int16_t)((buf[6]<<8)|buf[7]);
	gyro->x	=(int16_t)((buf[8]<<8)|buf[9]);
	gyro->y	=-(int16_t)((buf[10]<<8)|buf[11]);
	gyro->z	=-(int16_t)((buf[12]<<8)|buf[13]);
	*temperature=36.53f+(double)(temp/340.0f);
}

注：vector3f是一个含有三个浮点型变量的结构体

2.IIC模块（篇幅受限，不做介绍）
3.主函数

#include "Headfile.h"
#define LSB_ACCEL 16384	//加速度灵敏度常数 
#define LSB_GYRO  131  //角加速度灵敏度常数 
int fputc(int ch, FILE *f){UARTCharPut(UART0_BASE,ch);	return (ch);}//重新映射printf函数 
int fgetc(FILE *f) {int ch=UARTCharGet(UART0_BASE);	return (ch);}
vector3f wgyro,aaccel;
float tempr,xaxisA,yaxisA,zaxisA,xaxisB,yaxisB,zaxisB;
float XAxisREG,YAxisREG,ZAxisREG;
float GxREG,GyREG,GzREG;
float Gx,Gy,Gz;
float g_Gyro_xoffset = 2.665, g_Gyro_yoffset = -0.395, g_Gyro_zoffset = 0.825;//校正传感器角速度静止数据（校正后静止时角速度应为0） 
float yaw,pitch,roll;
int lasttime,nowtime;
void main_loop()
{
	MPU6050_Read_Data( &wgyro , &aaccel , &tempr );
	float Ax=aaccel.x/LSB_ACCEL;
	float Ay=aaccel.y/LSB_ACCEL;
	float Az=aaccel.z/LSB_ACCEL;
	float Angel_accX=atan(Ax/sqrt(Az*Az+Ay*Ay))*180/3.14;		//加速度计数据处理
    float Angel_accY=atan(Ay/sqrt(Ax*Ax+Az*Az))*180/3.14;
    float Angel_accZ=atan(Az/sqrt(Ax*Ax+Ay*Ay))*180/3.14;
	float gx=wgyro.x/LSB_GYRO;
	float gy=wgyro.y/LSB_GYRO;
	float gz=wgyro.z/LSB_GYRO;
	lasttime=nowtime;
	nowtime=millis();
	Gx=Gx+(gx-GxREG+g_Gyro_xoffset)*(nowtime-lasttime)/1000;	//陀螺仪数据处理
    Gy=Gy+(gy-GyREG+g_Gyro_yoffset)*(nowtime-lasttime)/1000;
    Gz=Gz+(gz-GzREG+g_Gyro_zoffset)*(nowtime-lasttime)/1000;
	GxREG=Gx,GyREG=Gy,GzREG=Gz;
	Gx=gx;
	XAxisREG+=Angel_accX;
	YAxisREG+=Angel_accY;
	ZAxisREG+=Angel_accZ;
	pitch=yijiehubu(Angel_accX,Gy,nowtime-lasttime);			//通过一阶互补滤波函数整合数据（博主这里也没搞懂，希望广大网友不吝赐教） 
	roll=yijiehubu(Angel_accY,Gx,nowtime-lasttime);
	yaw=Angel_accZ;
	printf("PITCH:%f ROLL%f YAW:%f\n",pitch,roll,yaw);			//姿态角输出 
}

int main(void)
{
  	HardWave_Init();
	printf("初始化完成");
  	while(1)
	{
		main_loop();
	}
}

p.s.:
加速度处理：利用重力矢量推测出当前传感器姿态，公式为：

Angel_accX=arctan(Ax/sqrt(Az*Az+Ay*Ay))*180/PI;
Angel_accY=arctan(Ax/sqrt(Ax*Ax+Az*Az))*180/PI;
Angel_accZ=arctan(Ax/sqrt(Ax*Ax+Ay*Ay))*180/PI;

角速度处理：对速度求积分得到姿态角

Gx=Gx+(gx-GxREG+g_Gyro_xoffset)*dt;
Gy=Gy+(gy-GyREG+g_Gyro_yoffset)*dt;
Gz=Gz+(gz-GzREG+g_Gyro_zoffset)*dt;

dt的获取：
使用millis函数获取当时的时间戳（以毫秒计），储存起来，与下一次采样时以相同方法获取的时间戳做差，这样就可以获得dt了（注意，dt数据需要除以1000转换为秒，而且需以浮点数储存，否则。。。会出很大的问题）

四、程序效果

注意: 本文的方法只能测出相对xyz坐标系的角度, 如果要算出pitch roll yaw角度需要进行四元数转换, 在下面的资源中附有测PRY角度相关工程

具体工程文件:

链接：https://pan.baidu.com/s/1biANvELF43nf-XEMi-boWg
提取码：5zc2