利用stm32f4单片机的mpu6050六轴加速度传感器获取姿态角详解（有代码）

mpu6050简介：

MPU-6000（6050）为全球首例整合性6轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时间轴之差的问题，减少了大量的封装空间。当连接到三轴磁强计时，MPU-60X0提供完整的9轴运动融合输出到其主I2C或SPI端口(SPI仅在MPU-6000上可用)。

MPU6050， 该芯片内部集成一个三轴加速度传感器和一个三轴陀螺仪，并且自带 DMP(Digital Motion Processor)，该传感器可以用于四轴飞行器的姿态控制和解算。

MPU-6000（6050）的角速度全格感测范围为±250、±500、±1000与±2000°/sec (dps)，可准确追踪快速与慢速动作，并且，用户可程式控制的加速器全格感测范围为±2g、±4g±8g与±16g。产品传输可透过最高至400kHz的IIC或最高达20MHz的SPI（MPU-6050没有SPI）。MPU-6000可在不同电压下工作，VDD供电电压介为2.5V±5%、3.0V±5%或3.3V±5%，逻辑接口VDDIO供电为1.8V± 5%（MPU6000仅用VDD）。MPU-6000的包装尺寸4x4x0.9mm(QFN)，在业界是革命性的尺寸。其他的特征包含内建的温度感测器、包含在运作环境中仅有±1%变动的振荡器。

其功能如图所示：

mup6050原理图：

STM32F4 开发板自带了 MPU6050 传感器。我们将使用 STM32F4 来驱动 MPU6050，读取其原始数据，并利用其自带的 DMP 实现姿态解算。MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口 ，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出完整的 9 轴融合演算数据。有了 DMP，我们可以使用 InvenSense 公司提供的运动处理资料库，非常方便的实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。

对于姿态角的理解：

姿态角和自由度相关联，在控制和机械领域广泛使用，它是用航向角、俯仰角和圆滚角三个欧拉角表示。

pitch是围绕X轴旋转，也叫做俯仰角。
yaw是围绕Y轴旋转，也叫偏航角。
roll是围绕Z轴旋转，也叫横滚角（翻滚角）。

pitch：
机体坐标系X轴与水平面的夹角。
当X轴的正半轴位于过坐标原点的水平面之上（抬头）时，俯仰角为正，否则为负。
pitch是围绕X轴旋转，也叫做俯仰角。

yaw：
机体坐标系xb轴在水平面上投影与地面坐标系xg轴（在水平面上，指向目标为正）之间的夹角，
由xg轴逆时针转至机体xb的投影线时，偏航角为正，即机头右偏航为正，
反之为负。
yaw是围绕Y轴旋转，也叫偏航角。

rool：
机体坐标系zb轴与通过机体xb轴的铅垂面间的夹角，机体向右滚为正，
反之为负。
roll是围绕Z轴旋转，也叫翻滚角。

DMP简介：

我们可以读出 MPU6050 的加速度传感器和角速度传感器的原始数据。不过这些原始数据，对想搞四轴之类的初学者来说，用处不大，我们期望得到的是姿态数据，也就是欧拉角：航向角（yaw）、横滚角（roll）和俯仰角（pitch）。有了这三个角，我们就可以得到当前四轴的姿态，这才是我们想要的结果。要得到欧拉角数据，就得利用我们的原始数据，进行姿态融合解算，这个比较复杂，知识点比较多，初学者 不易掌握。而 MPU6050 自带了数字运动处理器，即 DMP，并且， InvenSense提供了一个 MPU6050 的嵌入式运动驱动库，结合 MPU6050 的 DMP，可以将我们的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便的计算出欧拉角，从而得到 yaw、roll 和 pitch。使用内置的 DMP，大大简化了四轴的代码设计，且 MCU 不用进行姿态解算过程，大大降低了 MCU 的负担，从而有更多的时间去处理其他事件，提高系统实时性。使用 MPU6050 的 DMP 输出的四元数是 q30 格式的，也就是浮点数放大了 2 的 30 次方倍。

姿态角测量原理：

硬件设计：

采用 STM32F4 的 3 个普通 IO 连接 MPU6050， 本次设计基本流程：程序先初始化MPU6050 等外设，然后利用 DMP 库，初始化 MPU6050 及使能 DMP，最后，在死循环里面不停读取：温度传感器、加速度传感器、陀螺仪、 DMP 姿态解算后的欧拉角等数据，通过串口上报给上位机（温度不上报），利用上位机软件（ANO_Tech 匿名四轴上位机_V2.6.exe），可以实时显示 MPU6050 的传感器状态曲线，并显示 3D 姿态，可以通过 KEY0 按键开启/关闭数据上传功能。同时，在 LCD 模块上面显示温度和欧拉角等信息。 DS0 来指示程序正在运行。所要用到的硬件资源如下：
1） 指示灯 DS0
2） KEY0 按键
3） TFTLCD 模块
4） 串口
5） MPU6050
这里主要分析mup6050。

从上图可以看出， MPU6050 通过三根线与 STM32F4 开发板连接，其中 IIC 总线时和 24C02
以及 WM8978 共用，接在 PB8 和 PB9 上面。 MPU6050 的中断输出，连接在 STM32F4 的 PC0
脚，不过本例程我们并没有用到中断。另外， AD0 接的 GND，所以 MPU6050 的器件地址是：
0X68。

软件设计：

首先我们在工程中 HARDWARE 分组下首先添加了 IIC 支持的底层驱动文件 myiic.c 和源文件myiic.h，因为我们的 mpu6050 通信接口是 IIC。同时我们还增加了 mpu6050.c 源文件和对应的头文件 mpu6050 用来编写 mpu6050 相关的底层驱动。最后我们还 添 加 了 DMP 驱 动 库 代 码 到 我 们 实 验 工 程 ， DMP 驱 动 库 代 码 包 含 inv_mpu.c 和inv_mpu_dmp_motion_driver.c 两个源文件，以及几个头文件。

由于 mpu6050.c 里面代码比较多，这里我就不全部列出来了，仅介绍几个重要的函数。
首先是： MPU_Init，该函数代码如下：

//初始化 MPU6050
//返回值:0,成功 其他,错误代码
u8 MPU_Init(void)
{
u8 res;
IIC_Init();//初始化 IIC 总线
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X80);//复位 MPU6050
delay_ms(100);
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X00);//唤醒 MPU6050
MPU_Set_Gyro_Fsr(3); //陀螺仪传感器,±2000dps
MPU_Set_Accel_Fsr(0); //加速度传感器,±2g
MPU_Set_Rate(50); //设置采样率 50Hz
MPU_Write_Byte(MPU_INT_EN_REG,0X00); //关闭所有中断
MPU_Write_Byte(MPU_USER_CTRL_REG,0X00); //I2C 主模式关闭
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)//器件 ID 正确
{
MPU_Write_Byte(MPU_PWR_MGMT1_REG,0X01);//设置 CLKSEL,PLL X 轴参考
MPU_Write_Byte(MPU_PWR_MGMT2_REG,0X00);//加速度与陀螺仪都工作
MPU_Set_Rate(50); //设置采样率为 50Hz
}else return 1;
return 0;
}

该函数对 MPU6050 进行初始化，该函数执行成功后，便可以读取传感器数据了。

然后再看 MPU_Get_Temperature、 MPU_Get_Gyroscope 和 MPU_Get_Accelerometer 等三个
函数，源码如下：

//得到温度值
//返回值:温度值(扩大了 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:陀螺仪 x,y,z 轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
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;;
}
//得到加速度值(原始值)
//gx,gy,gz:陀螺仪 x,y,z 轴的原始读数(带符号)
//返回值:0,成功
// 其他,错误代码
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;;
}

其中 MPU_Get_Temperature 用于获取 MPU6050 自带温度传感器的温度值，然后MPU_Get_Gyroscope 和 MPU_Get_Accelerometer 分别用于读取陀螺仪和加速度传感器的原
始数据。

最后看 MPU_Write_Len 和 MPU_Read_Len 这两个函数，代码如下

//IIC 连续写
//addr:器件地址 reg:寄存器地址
//len:写入长度 buf:数据区
//返回值:0,正常 其他,错误代码
u8 MPU_Write_Len(u8 addr,u8 reg,u8 len,u8 *buf)
{
u8 i;
IIC_Start();
IIC_Send_Byte((addr<<1)|0);//发送器件地址+写命令
if(IIC_Wait_Ack()){IIC_Stop();return 1;}//等待应答
IIC_Send_Byte(reg); //写寄存器地址
IIC_Wait_Ack(); //等待应答
for(i=0;i

MPU_Write_Len 用于指定器件和地址，连续写数据，可用于实现 DMP 部分的： i2c_write函数。而 MPU_Read_Len 用于指定器件和地址，连续读数据，可用于实现 DMP 部分的： i2c_read函数。 DMP 移植部分的 4 个函数，这里就实现了 2 个，剩下的 delay_ms 就直接采用我们 delay.c里面的 delay_ms 实现， get_ms 则直接提供一个空函数即可。

关于 mpu6050.c 就介绍到这。

最后看看 main.c 代码如下：

//串口 1 发送 1 个字符
//c:要发送的字符
void usart1_send_char(u8 c)
{
while(USART_GetFlagStatus(USART1,USART_FLAG_TC)==RESET);
USART_SendData(USART1,c);
}
//传送数据给匿名四轴上位机软件(V2.6 版本)
//fun:功能字. 0XA0~0XAF
//data:数据缓存区,最多 28 字节!!
//len:data 区有效数据个数
void usart1_niming_report(u8 fun,u8*data,u8 len)
{
u8 send_buf[32],i;
if(len>28)return; //最多 28 字节数据
send_buf[len+3]=0;//校验数置零
send_buf[0]=0X88;//帧头
send_buf[1]=fun; //功能字
send_buf[2]=len; //数据长度
for(i=0;i>8)&0XFF; tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF; tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF; tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF; tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF; tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF; tbuf[11]=gyroz&0XFF;
usart1_niming_report(0XA1,tbuf,12);//自定义帧,0XA1
}
//通过串口 1 上报结算后的姿态数据给电脑
//aacx,aacy,aacz:x,y,z 三个方向上面的加速度值
//gyrox,gyroy,gyroz:x,y,z 三个方向上面的陀螺仪值
//roll:横滚角.单位 0.01 度。 -18000 -> 18000 对应 -180.00 -> 180.00 度
//pitch:俯仰角.单位 0.01 度。 -9000 - 9000 对应 -90.00 -> 90.00 度
//yaw:航向角.单位为 0.1 度 0 -> 3600 对应 0 -> 360.0 度
void usart1_report_imu(short aacx,short aacy,short aacz,short gyrox,short gyroy,
short gyroz,short roll,short pitch,short yaw)
{
u8 tbuf[28];
u8 i;
for(i=0;i<28;i++)tbuf[i]=0;//清 0
tbuf[0]=(aacx>>8)&0XFF; tbuf[1]=aacx&0XFF;
tbuf[2]=(aacy>>8)&0XFF; tbuf[3]=aacy&0XFF;
tbuf[4]=(aacz>>8)&0XFF; tbuf[5]=aacz&0XFF;
tbuf[6]=(gyrox>>8)&0XFF; tbuf[7]=gyrox&0XFF;
tbuf[8]=(gyroy>>8)&0XFF; tbuf[9]=gyroy&0XFF;
tbuf[10]=(gyroz>>8)&0XFF; tbuf[11]=gyroz&0XFF;
tbuf[18]=(roll>>8)&0XFF; tbuf[19]=roll&0XFF;
tbuf[20]=(pitch>>8)&0XFF; tbuf[21]=pitch&0XFF;
tbuf[22]=(yaw>>8)&0XFF; tbuf[23]=yaw&0XFF;
usart1_niming_report(0XAF,tbuf,28);//飞控显示帧,0XAF
}
int main(void)
{
u8 t=0,report=1; //默认开启上报
u8 key;
float pitch,roll,yaw; //欧拉角
short aacx,aacy,aacz; //加速度传感器原始数据
short gyrox,gyroy,gyroz;//陀螺仪原始数据
short temp; //温度
NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);//设置系统中断优先级分组 2
delay_init(168); //初始化延时函数
uart_init(500000); //初始化串口波特率为 500000
LED_Init(); //初始化 LED
KEY_Init(); //初始化按键
LCD_Init(); //LCD 初始化
MPU_Init(); //初始化 MPU6050
POINT_COLOR=RED;//设置字体为红色
LCD_ShowString(30,50,200,16,16,"Explorer STM32F4");
LCD_ShowString(30,70,200,16,16,"MPU6050 TEST");
LCD_ShowString(30,90,200,16,16,"ATOM@ALIENTEK");
LCD_ShowString(30,110,200,16,16,"2014/5/9");
while(mpu_dmp_init())
{ LCD_ShowString(30,130,200,16,16,"MPU6050 Error"); delay_ms(200);
LCD_Fill(30,130,239,130+16,WHITE); delay_ms(200);
}
LCD_ShowString(30,130,200,16,16,"MPU6050 OK");
LCD_ShowString(30,150,200,16,16,"KEY0:UPLOAD ON/OFF");
POINT_COLOR=BLUE;//设置字体为蓝色
LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
LCD_ShowString(30,200,200,16,16," Temp: . C");
LCD_ShowString(30,220,200,16,16,"Pitch: . C");
LCD_ShowString(30,240,200,16,16," Roll: . C");
LCD_ShowString(30,260,200,16,16," Yaw : . C");
while(1)
{
key=KEY_Scan(0);
if(key==KEY0_PRES)
{
report=!report;
if(report)LCD_ShowString(30,170,200,16,16,"UPLOAD ON ");
else LCD_ShowString(30,170,200,16,16,"UPLOAD OFF");
}
if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
{
temp=MPU_Get_Temperature(); //得到温度值
MPU_Get_Accelerometer(&aacx,&aacy,&aacz); //得到加速度传感器数据
MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz); //得到陀螺仪数据
if(report)mpu6050_send_data(aacx,aacy,aacz,gyrox,gyroy,gyroz);
//用自定义帧发送加速度和陀螺仪原始数据
if(report)usart1_report_imu(aacx,aacy,aacz,gyrox,gyroy,gyroz,(int)(roll*100),
(int)(pitch*100),(int)(yaw*10));
if((t%10)==0)
{
if(temp<0)
{
LCD_ShowChar(30+48,200,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,200,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,200,temp/100,3,16); //显示整数部分
LCD_ShowNum(30+48+40,200,temp%10,1,16); //显示小数部分
temp=pitch*10;
if(temp<0)
{
LCD_ShowChar(30+48,220,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,220,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,220,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,220,temp%10,1,16); //显示小数部分
temp=roll*10;
if(temp<0)
{
LCD_ShowChar(30+48,240,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,240,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,240,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,240,temp%10,1,16); //显示小数部分
temp=yaw*10;
if(temp<0)
{
LCD_ShowChar(30+48,260,'-',16,0); //显示负号
temp=-temp; //转为正数
}else LCD_ShowChar(30+48,260,' ',16,0); //去掉负号
LCD_ShowNum(30+48+8,260,temp/10,3,16); //显示整数部分
LCD_ShowNum(30+48+40,260,temp%10,1,16); //显示小数部分
t=0; LED0=!LED0;//LED 闪烁
}
}
t++;
}
}

此部分代码除了 main 函数，还有几个函数，用于上报数据给上位机软件，利用上位机软件显示传感器波形，以及 3D 姿态显示，有助于更好的调试 MPU6050。上位机软件推荐使用： ANO_Tech匿名四轴上位机_V2.6.exe。 usart1_niming_report 函数用于将数据打包、计算校验和，然后上报给匿名四轴上位机软件。mpu6050_send_data 函数用于上报加速度和陀螺仪的原始数据，可用于波形显示传感器数据，通过 A1 自定义帧发送。而 usart1_report_imu 函数，则用于上报飞控显示帧，可以实时 3D 显示
MPU6050 的姿态，传感器数据等。

这里， main 函数是比较简单的，大家看代码即可，不过需要注意的是，为了高速上传数据，
这里将串口 1 的波特率设置为 500Kbps 了，测试的时候要注意下。

最后，将 MPU_Write_Byte、 MPU_Read_Byte 和 MPU_Get_Temperature 等三个函数加入 USMART 控制，这样，我们就可以通过串口调试助手，改写和读取 MPU6050 的寄存器数据了，并可以读取温度传感器的值，方便大家调试（注意在 USMART 调试的时候，最好通过按KEY0，先关闭数据上传功能，否则会受到很多乱码，妨碍调试）。
至此，我们的软件设计部分就结束了。

下载验证：

在代码编译成功之后，我们通过下载代码到 ALIENTEK 探索者 STM32F4 开发板上，可以
看到 LCD 显示如图 ：

屏幕显示了 MPU6050 的温度、俯仰角（pitch）、横滚角（roll）和航向角（yaw）的数值。
然后，可以晃动开发板，看看各角度的变化。

另外，通过按 KEY0 可以开启或关闭数据上报，开启状态下，我们可以打开： ANO_Tech
匿名四轴上位机_V2.6.exe（该软件双击后，会弹出一个蓝色的小界面，直接关闭即可。然后才
会进入主界面），这个软件，接收 STM32F4 上传的数据，从而图形化显示传感器数据以及飞行
姿态，如图 ：
传感器数据波形显示：

飞控状态显示：

也可以通过串口调试助手，读取数据，保存数据，对数据进行后续处理。