STM32—驱动六轴MPU6050输出欧拉角_六轴加速度转欧拉角

一.MPU6050介绍

1.MPU6050与陀螺仪、加速度计的关系：

MPU6050是InvenSense公司推出的一款全球首款的整合性9轴运动处理传感器，其最大的特色就是：消除了陀螺仪和加速度计的误差，将陀螺仪和加速度计组合在一起，而且缩小了空间。关于陀螺仪、加速度计和MPU6050之间的关系之前在一篇博客中已经做了介绍：博客链接

2.整体概括

MPU6050内部整合了三轴MEMS陀螺仪、三轴MEMS加速度计以及一个可扩展的数字运动处理器DMP，而且还可以连接一个第三方数字传感器（比如：磁力计），这样的话，就可以通过IIC接口输出一个9轴信号。
更加方便的是，有了DMP，可以结合InvenSense公司提供的运动处理资料库，实现姿态解算。通过自带的DMP，可以通过IIC接口输出9轴融合演算的数据，大大降低了运动处理运算对操作系统的负荷，同时也降低了开发难度。

特点：
① 以数字形式输出 6 轴或 9 轴（需外接磁传感器）的旋转矩阵、四元数(quaternion)、欧
拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）
② 具有 131 LSBs/° /sec 敏感度与全格感测范围为±250、±500、±1000 与±2000° /sec
的 3 轴角速度感测器(陀螺仪)
③ 集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器
④ 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移
⑤ 自带数字运动处理(DMP: Digital Motion Processing)引擎可减少 MCU 复杂的融合演算
数据、感测器同步化、姿势感应等的负荷
⑥ 内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求
⑦ 自带一个数字温度传感器
⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS
⑨ 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降
中断、 high-G 中断、零动作感应、触击感应、摇动感应功能
⑩ VDD 供电电压为 2.5V±5%、 3.0V±5%、 3.3V±5%； VLOGIC 可低至 1.8V± 5%
⑪ 陀螺仪工作电流： 5mA，陀螺仪待机电流： 5uA； 加速器工作电流： 500uA，加速器省
电模式电流： 40uA@10Hz
⑫ 自带 1024 字节 FIFO，有助于降低系统功耗
⑬ 高达 400Khz 的 IIC 通信接口
⑭ 超小封装尺寸： 4x4x0.9mm（QFN）

其检测轴如图所示：

3.引脚说明

如图，MPU6050一共有8个引脚，实际上输出六轴数据时，只用了5个：VCC、GND、SCL、SDA、AD0。下面介绍一下引脚：

• VCC：供电，3.3V即可
• GND：接地
• SCL：连接MCU的IIC时钟接口
• SAD：连接MCU的IIC数据接口
• XCL：连接外部设备的IIC时钟接口
• XAD：连接外部设备的IIC数据接口
• AD0：地址控制引脚（控制地址的最低位）
• INT：中断触发接口（不用）

XCL、XDA只有在连接外部设备（比如磁力计的时候才用），AD0用来控制MPU6050的地址，如果AD0低电平，地址就是0X68；如果AD0高电平，地址就是0X69。

4.基本配置及相关寄存器

MCU与MPU6050的通信是建立在IIC通信机制上的，在IIC的基础上，可以实现对MPU6050的寄存器的操作，而MPU6050的运作就是过对寄存器进行读写。所以，了解相关的寄存器和对寄存器的操作是很有必要的。MPU6050的寄存器相关资料都可以在数据手册中查到，下面介绍一下几个重要的寄存器：

电源管理寄存器1

地址：0X68
主要位的功能：

• DEVICE_RESET：控制复位，1表示复位，复位后会自动清零；
• SLEEP：控制MPU6050工作模式，1表示睡眠模式，0表示正常工作模式，复位后改位为1，要手动将改位清零；
• TEMP_DIS：使能温度传感器位，0代表使能；
• CLKSEL[2:0]：选择系统时钟源，一般采用PLL_X轴陀螺作为参考，具体的时钟源选择及相应位的值如图：

陀螺仪配置寄存器

地址：0X1B

主要位的功能：

• FS_SEL[1:0]：设置陀螺仪满量程范围：为0代表±250° /S、为1代表±500° /S、为2代表±1000° /S为3代表±2000° /S；

陀螺仪的分辨率是16位，所以在最大量程下灵敏度为： 65536/4000=16.4LSB/(° /S)。

加速度计配置寄存器

地址：0X1C

主要位的功能：

• AFS_SEL[1:0]：设置加速度计满量程范围：为 0代表±2g、为1代表±4g、为 2代表±8g、为 3代表±16g；

FIFO使能寄存器

地址：0X23

用来控制FIFO功能，相应位对应着相应的传感器FIFO功能，为0代表禁止，为1代表使能。注意：加速度传感器的三个轴的FIFO功能由一个位ACCEL_FIFO_EN控制。在简单读取传感器数据的情况下可以不使用FIFO。

陀螺仪采样率分频寄存器

地址：0X19

改寄存器用来设置MPU6050陀螺仪的采样频率，与之相关的是陀螺仪的输出频率，俩者关系是：采样频率 = 陀螺仪输出频率 / (1+SMPLRT_DIV)
陀螺仪输出频率与数字低通滤波器（DLPF）有关，DLPF滤波频率一般设置为采样率的一半。

温度传感器寄存器

地址：高八位0X41、低八位0X42
直接通过读取寄存器中的值来得到温度数据，温度换算公式为：
Temperature = 36.53 + regval/340

二.代码详解

1.框架

我是用STM32驱动MPU6050，MPU6050输出原始的六轴数据，经过DMP处理（有库）得到四元数，再由四元数算出欧拉角：yaw、roll、pitch。由串口打印在电脑屏幕上。

• 首先，要做底层的IIC驱动，用来和MPU6050建立通信，我在mpu_iic.c中实现了；
• 然后，有了底层的驱动，就要写一些函数来与MPU6050交流了（通过读写寄存器），还可以写入命令、配置MPU6050、读取原始数据等等，这些操作我都写在mpu6050.c中，当然在mpu6050.h头文件中还包含了MPU各寄存器地址和相关指令。
• 通过mpu6050.c的实现，就可以读出原始六轴数据，下一步就是通过DMP将原始数据转换为四元数，这一步的DMP算法我水平有限，只能移植InvenSense公司提供的例程。关于移植DMP算法，由于DMP算法本质也是对MPU6050的操作，所以我们只需要向移植过来的算法提供：对MPU6050寄存器执行读和写的函数接口即可，最后通过移植过来的函数直接读出四元数！
• 然后，就是将四元数转换为欧拉角了，这个比较简单，一个函数就可以实现。
• 最后，打印串口到屏幕。

下面给出各函数文件

2.mpu_iic.c/mpu_iic.h

mpu_iic.h
主要是宏定义对引脚电平的操作和进行函数声明。

#ifndef __MPU_IIC_H
#define __MPU_IIC_H

#include "stm32f10x.h"
#include "delay.h"

/* 宏定义引脚电平操作函数 */
#define MPU_SDA_IN()  {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=8<<12;}
#define MPU_SDA_OUT() {GPIOB->CRH&=0XFFFF0FFF;GPIOB->CRH|=3<<12;}  


#define MPU_IIC_SDA_1() GPIO_SetBits( GPIOB, GPIO_Pin_11 )
#define MPU_IIC_SDA_0() GPIO_ResetBits( GPIOB, GPIO_Pin_11 )

#define MPU_IIC_SCL_1() GPIO_SetBits( GPIOB, GPIO_Pin_10 )
#define MPU_IIC_SCL_0() GPIO_ResetBits( GPIOB, GPIO_Pin_10 )

#define MPU_IIC_AD0_1() GPIO_SetBits( GPIOA, GPIO_Pin_15 )
#define MPU_IIC_AD0_0() GPIO_ResetBits( GPIOA, GPIO_Pin_15 )

#define MPU_IIC_SDA_READ() GPIO_ReadInputDataBit( GPIOB, GPIO_Pin_11 )

#define MPU_IIC_Delay() delay_us(2)

/* 函数声明 */
void MPU_IIC_Init( void );
void MPU_IIC_Start( void );
void MPU_IIC_Stop( void );
uint8_t MPU_IIC_Wait_Ack( void );
void MPU_IIC_Ack( void );
void MPU_IIC_NAck( void );
void MPU_IIC_Send_Byte( uint8_t data );
uint8_t MPU_IIC_Read_Byte( uint8_t ack );

#endif

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36

mpu_iic.c
通过软件模拟IIC的代码，没什么好说的。

#include "mpu_iic.h"
#include "usart.h"

/*
IIC接口引脚配置
SDA:PB11
SCL:PB10
AD0:PB2
*/
void MPU_IIC_Init( void )
{
	GPIO_InitTypeDef  GPIO_InitStruct;
	
	RCC_APB2PeriphClockCmd( RCC_APB2Periph_GPIOB, ENABLE );
	
	GPIO_InitStruct.GPIO_Speed = GPIO_Speed_50MHz;
	GPIO_InitStruct.GPIO_Mode = GPIO_Mode_Out_PP;
	GPIO_InitStruct.GPIO_Pin = GPIO_Pin_10|GPIO_Pin_11;
	GPIO_Init( GPIOB, &GPIO_InitStruct );
	
	MPU_IIC_SDA_1();
	MPU_IIC_SCL_1();

}


void MPU_IIC_Start( void )
{
	MPU_SDA_OUT();
	
	MPU_IIC_SDA_1();
	MPU_IIC_SCL_1();
	delay_us(2);
	MPU_IIC_SDA_0();
	delay_us(2);
	MPU_IIC_SCL_0();
}

void MPU_IIC_Stop( void )
{
	MPU_SDA_OUT();
	
	MPU_IIC_SDA_0();
	MPU_IIC_SCL_1();
	delay_us(2);
	MPU_IIC_SDA_1();
	MPU_IIC_SCL_1();
	delay_us(2);
	
}

/* 由从设备在SCL为高电平的时候拉低SDA作为应答 
返回值：1：未应答
        0：已应答
*/
uint8_t MPU_IIC_Wait_Ack( void )
{
	uint8_t count;
	MPU_SDA_IN();
	
	MPU_IIC_SCL_1();
	delay_us(2);
	MPU_IIC_SDA_1();
	delay_us(2);
	
	while( MPU_IIC_SDA_READ()==1 )
	{
		count++;
		if( count>250 )
		{
			MPU_IIC_Stop();
			return 1;
		}
	}	
	MPU_IIC_SCL_0();
	return 0;
}


void MPU_IIC_Ack( void )
{
	
	MPU_IIC_SCL_0();
	MPU_SDA_OUT();
	MPU_IIC_SDA_0();
	delay_us(2);
	MPU_IIC_SCL_1();
	delay_us(2);
	MPU_IIC_SCL_0();
	
}


void MPU_IIC_NAck( void )
{
	
	
	MPU_IIC_SCL_0();
	MPU_SDA_OUT();
	MPU_IIC_SDA_1();
	delay_us(2);
	MPU_IIC_SCL_1();
	delay_us(2);
	MPU_IIC_SCL_0();
}


/* 发送一个字节数据，高位先行 */
void MPU_IIC_Send_Byte( uint8_t data )
{
	uint8_t t;
	MPU_SDA_OUT();
	
	MPU_IIC_SCL_0();
	for( t=0;t<8;t++ )
	{
		if( ((data&0x80)>>7)==1 )
			MPU_IIC_SDA_1();
		else
			MPU_IIC_SDA_0();
		data<<=1;
		MPU_IIC_SCL_1();
		delay_us(2);
		MPU_IIC_SCL_0();
		delay_us(2);
	}
}


/* 读取一个字节，ack=1时，读取完成后主机发送应答 */
uint8_t MPU_IIC_Read_Byte( uint8_t ack )
{
	uint8_t t,data=0;
	MPU_SDA_IN();
	for( t=0;t<8;t++ )
	{
		MPU_IIC_SCL_0();
		delay_us(2);//等待SDA的变化
		MPU_IIC_SCL_1();
		
		data<<=1;//必须在读取前面，因为之后一位读取后就不再移位
		if( MPU_IIC_SDA_READ()==1 )
			data++;
		
		delay_us(2);//等待SDA的变化
		
	}

	if( !ack )
		MPU_IIC_NAck();//发送nACK
    else
        MPU_IIC_Ack(); //发送ACK 
	return data;
}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156

3.mpu6050.c/mpu6050.h

mpu6050.h
主要是定义MPU相关寄存器的地址，和进行函数声明。

#ifndef __MPU6050_H
#define __MPU6050_H

#include "stm32f10x.h"
#include "mpu_iic.h"


/* AD0接地，MPU6050的IIC地址为0x68   接3.3V就为0x69*/
#define MPU_ADDR				0X68

/************** MPU6050相关寄存器地址 *********************/
#define MPU_ACCEL_OFFS_REG		0X06	//accel_offs寄存器,可读取版本号,寄存器手册未提到
#define MPU_PROD_ID_REG			0X0C	//prod id寄存器,在寄存器手册未提到
#define MPU_SELF_TESTX_REG		0X0D	//自检寄存器X
#define MPU_SELF_TESTY_REG		0X0E	//自检寄存器Y
#define MPU_SELF_TESTZ_REG		0X0F	//自检寄存器Z
#define MPU_SELF_TESTA_REG		0X10	//自检寄存器A
#define MPU_SAMPLE_RATE_REG		0X19	//采样频率分频器
#define MPU_CFG_REG				0X1A	//配置寄存器
#define MPU_GYRO_CFG_REG		0X1B	//陀螺仪配置寄存器
#define MPU_ACCEL_CFG_REG		0X1C	//加速度计配置寄存器
#define MPU_MOTION_DET_REG		0X1F	//运动检测阀值设置寄存器
#define MPU_FIFO_EN_REG			0X23	//FIFO使能寄存器
#define MPU_I2CMST_CTRL_REG		0X24	//IIC主机控制寄存器
#define MPU_I2CSLV0_ADDR_REG	0X25	//IIC从机0器件地址寄存器
#define MPU_I2CSLV0_REG			0X26	//IIC从机0数据地址寄存器
#define MPU_I2CSLV0_CTRL_REG	0X27	//IIC从机0控制寄存器
#define MPU_I2CSLV1_ADDR_REG	0X28	//IIC从机1器件地址寄存器
#define MPU_I2CSLV1_REG			0X29	//IIC从机1数据地址寄存器
#define MPU_I2CSLV1_CTRL_REG	0X2A	//IIC从机1控制寄存器
#define MPU_I2CSLV2_ADDR_REG	0X2B	//IIC从机2器件地址寄存器
#define MPU_I2CSLV2_REG			0X2C	//IIC从机2数据地址寄存器
#define MPU_I2CSLV2_CTRL_REG	0X2D	//IIC从机2控制寄存器
#define MPU_I2CSLV3_ADDR_REG	0X2E	//IIC从机3器件地址寄存器
#define MPU_I2CSLV3_REG			0X2F	//IIC从机3数据地址寄存器
#define MPU_I2CSLV3_CTRL_REG	0X30	//IIC从机3控制寄存器
#define MPU_I2CSLV4_ADDR_REG	0X31	//IIC从机4器件地址寄存器
#define MPU_I2CSLV4_REG			0X32	//IIC从机4数据地址寄存器
#define MPU_I2CSLV4_DO_REG		0X33	//IIC从机4写数据寄存器
#define MPU_I2CSLV4_CTRL_REG	0X34	//IIC从机4控制寄存器
#define MPU_I2CSLV4_DI_REG		0X35	//IIC从机4读数据寄存器

#define MPU_I2CMST_STA_REG		0X36	//IIC主机状态寄存器
#define MPU_INTBP_CFG_REG		0X37	//中断/旁路设置寄存器
#define MPU_INT_EN_REG			0X38	//中断使能寄存器
#define MPU_INT_STA_REG			0X3A	//中断状态寄存器

#define MPU_ACCEL_XOUTH_REG		0X3B	//加速度值,X轴高8位寄存器
#define MPU_ACCEL_XOUTL_REG		0X3C	//加速度值,X轴低8位寄存器
#define MPU_ACCEL_YOUTH_REG		0X3D	//加速度值,Y轴高8位寄存器
#define MPU_ACCEL_YOUTL_REG		0X3E	//加速度值,Y轴低8位寄存器
#define MPU_ACCEL_ZOUTH_REG		0X3F	//加速度值,Z轴高8位寄存器
#define MPU_ACCEL_ZOUTL_REG		0X40	//加速度值,Z轴低8位寄存器

#define MPU_TEMP_OUTH_REG		0X41	//温度值高八位寄存器
#define MPU_TEMP_OUTL_REG		0X42	//温度值低8位寄存器

#define MPU_GYRO_XOUTH_REG		0X43	//陀螺仪值,X轴高8位寄存器
#define MPU_GYRO_XOUTL_REG		0X44	//陀螺仪值,X轴低8位寄存器
#define MPU_GYRO_YOUTH_REG		0X45	//陀螺仪值,Y轴高8位寄存器
#define MPU_GYRO_YOUTL_REG		0X46	//陀螺仪值,Y轴低8位寄存器
#define MPU_GYRO_ZOUTH_REG		0X47	//陀螺仪值,Z轴高8位寄存器
#define MPU_GYRO_ZOUTL_REG		0X48	//陀螺仪值,Z轴低8位寄存器

#define MPU_I2CSLV0_DO_REG		0X63	//IIC从机0数据寄存器
#define MPU_I2CSLV1_DO_REG		0X64	//IIC从机1数据寄存器
#define MPU_I2CSLV2_DO_REG		0X65	//IIC从机2数据寄存器
#define MPU_I2CSLV3_DO_REG		0X66	//IIC从机3数据寄存器

#define MPU_I2CMST_DELAY_REG	0X67	//IIC主机延时管理寄存器
#define MPU_SIGPATH_RST_REG		0X68	//信号通道复位寄存器
#define MPU_MDETECT_CTRL_REG	0X69	//运动检测控制寄存器
#define MPU_USER_CTRL_REG		0X6A	//用户控制寄存器
#define MPU_PWR_MGMT1_REG		0X6B	//电源管理寄存器1
#define MPU_PWR_MGMT2_REG		0X6C	//电源管理寄存器2 
#define MPU_FIFO_CNTH_REG		0X72	//FIFO计数寄存器高八位
#define MPU_FIFO_CNTL_REG		0X73	//FIFO计数寄存器低八位
#define MPU_FIFO_RW_REG			0X74	//FIFO读写寄存器
#define MPU_DEVICE_ID_REG		0X75	//器件ID寄存器




/* 函数声明 */
uint8_t MPU_Read_Byte( uint8_t reg );
uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data );
uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf );
uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf );

uint8_t MPU_Init( void );


uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr );
uint8_t MPU_Set_Accel_Fsr( uint8_t fsr );
uint8_t MPU_Set_LPF( uint16_t lpf );
uint8_t MPU_Set_Rate( uint16_t rate );
short MPU_Get_Temperature( void );
uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz );
uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az );

#endif

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102

mpu6050.c
最为重要的一部分代码，包括了对MPU6050的一系列基本配置和读取原始数据的操作，代码都注解的很详细了。
AD0引脚用了PA15，所以要GPIO_Remap_SWJ_JTAGDisable，换个引脚也可以。

#include "mpu6050.h"
#include "usart.h"

/**
  * @brief  ：初始化MPU
  * @param  ：None
  * @retval ：0：初始化完成
*/
uint8_t MPU_Init( void )
{
	uint8_t k=1;
	
	uint8_t res;
	
	 GPIO_InitTypeDef  GPIO_InitStructure;
	
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO,ENABLE);//使能AFIO时钟 
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);//先使能外设IO PORTA时钟 
	
  GPIO_InitStructure.GPIO_Pin = GPIO_Pin_15;	 // 端口配置
  GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP; 		 //推挽输出
  GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;		 //IO口速度为50MHz
  GPIO_Init(GPIOA, &GPIO_InitStructure);					 //根据设定参数初始化GPIOA

	GPIO_PinRemapConfig(GPIO_Remap_SWJ_JTAGDisable,ENABLE);
	MPU_IIC_AD0_0();
	MPU_IIC_Init();
	/* 设置MPU6050地址为0X68,并初始化IIC总线 */
	
	
	/* MPU_PWR_MGMT1_REG：电源管理寄存器 */
	MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X80 );//复位MPU6050
//	printf("读寄存器值：%02X\n",MPU_Read_Byte( MPU_PWR_MGMT1_REG ));
	delay_ms(100);
	MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X00 );//唤醒MPU6050
	
	/* 设置陀螺仪量程：±2000dps
	 设置加速度计量程：±2g
	     设置采样频率：50Hz（低通滤波器频率100Hz）   */
	MPU_Set_Gyro_Fsr( 3 );
	MPU_Set_Accel_Fsr( 0 );
	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 ); //读取MPU6050ID
  printf("ID:%X\n",res);
	/* 确认ID */
	if( res==MPU_ADDR )
	{
		MPU_Write_Byte( MPU_PWR_MGMT1_REG, 0X01 );//以PLL X轴作为时钟参考
		MPU_Write_Byte( MPU_PWR_MGMT2_REG, 0X00 );//使能陀螺仪和加速度计
		MPU_Set_Rate( 50 );
		return 0;
	}
	else
		return 1;
	
}



/**
  * @brief  ：读一个寄存器的值（8位）
  * @param  ：reg：寄存器地址
  * @retval ：读到寄存器的数据
*/
uint8_t MPU_Read_Byte( uint8_t reg )
{
	uint8_t data;
	
	MPU_IIC_Start();
	/* 发送MPU6050器件地址，还有写命令（最低位是0） */
	MPU_IIC_Send_Byte( (MPU_ADDR<<1)|0 );
	MPU_IIC_Wait_Ack();
	/* 写入寄存器地址 */
	MPU_IIC_Send_Byte( reg );
	MPU_IIC_Wait_Ack();
	
	MPU_IIC_Start();
	/* 对MPU6050发送读命令 */
	MPU_IIC_Send_Byte( (MPU_ADDR<<1)|1 );
	MPU_IIC_Wait_Ack();
	/* 读取寄存器的值 */
	data = MPU_IIC_Read_Byte( 0 );
	MPU_IIC_Stop();
	return data;
}


/**
  * @brief  ：在指定寄存器中写入数据（8位）
  * @param  ：reg：寄存器地址
             data：要写入寄存器的数据
  * @retval ：正常返回0
*/
uint8_t MPU_Write_Byte( uint8_t reg, uint8_t data )
{
	MPU_IIC_Start();
	/* 发送MPU6050器件地址，还有写命令（最低位是0） */
	MPU_IIC_Send_Byte( (MPU_ADDR<<1)|0 );
	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;
}



/**
  * @brief  ：连续读取寄存器中的数据
  * @param  ：reg：寄存器地址
              len：要读取数据的长度（以Byte为单位）
             *buf：存储读取到的数据
  * @retval ：正常返回0
*/
uint8_t MPU_Read_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf )
{
	
	MPU_IIC_Start();
	/* 发送MPU6050器件地址，还有写命令（最低位是0） */
	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();
	/* 对MPU6050发送读命令 */
	MPU_IIC_Send_Byte( (MPU_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;
}



/**
  * @brief  ：连续在寄存器中写入数据
  * @param  ：reg：寄存器地址
              len：要写入数据的长度（以Byte为单位）
             *buf：要写入寄存器的数据
  * @retval ：正常返回0
*/
uint8_t MPU_Write_Continue( uint8_t addr, uint8_t reg, uint8_t len, uint8_t *buf )
{
	uint8_t i;
	
	MPU_IIC_Start();
	/* 发送MPU6050器件地址，还有写命令（最低位是0） */
	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();
	
	i=0;
	while( len )
	{
		MPU_IIC_Send_Byte( buf[i] );
		i++;
		if( MPU_IIC_Wait_Ack() )
		{
			MPU_IIC_Stop();
			return 1;
		}
		len--;
	}
	
	MPU_IIC_Stop();
		return 0;
}




/**
  * @brief  ：设置陀螺仪量程
  * @param  ：fsr:0：±250dps
                  1：±500dps
									2：±1000dps
									3：±2000dps
  * @retval ：0：设置成功
              1：设置失败
*/
uint8_t MPU_Set_Gyro_Fsr( uint8_t fsr )
{
	return MPU_Write_Byte( MPU_GYRO_CFG_REG, fsr<<3 );
}


/**
  * @brief  ：设置加速度计量程
  * @param  ：fsr:0：±2g
                  1：±4g
									2：±8g
									3：±16g
  * @retval ：0：设置成功
              1：设置失败
*/
uint8_t MPU_Set_Accel_Fsr( uint8_t fsr )
{
	return MPU_Write_Byte( MPU_ACCEL_CFG_REG, fsr<<3 );
}


/**
  * @brief  ：设置低通滤波器频率
  * @param  ：Hz：频率
  * @retval ：0：设置成功
              1：设置失败
*/
uint8_t MPU_Set_LPF( uint16_t lpf )
{
	uint8_t 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);
}


/**
  * @brief  ：设置采样频率
  * @param  ：Hz：4~1000Hz
  * @retval ：0：设置成功
              1：设置失败
*/
uint8_t MPU_Set_Rate( uint16_t rate )
{
	uint8_t 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);	//自动设置LPF为采样率的一半
}


/**
  * @brief  ：获取温度值
  * @param  ：None
  * @retval ：扩大了100倍的温度值(实际上为了保留俩位小数)
*/
short MPU_Get_Temperature( void )
{
	uint8_t buf[2];
	uint16_t raw;//存储原始温度值
	float temp;
	
	/* MPU_TEMP_OUTH_REG：温度值的高八位寄存器0X41
	   MPU_TEMP_OUTL_REG：温度值的低八位寄存器0X42
	   连续读出16位温度值*/
	MPU_Read_Continue( MPU_ADDR, MPU_TEMP_OUTH_REG, 2, buf );
	/* 读取原始温度值 */
	raw = ( ( uint16_t )buf[0]<<8 )|buf[1];
	/* 转换温度值 */
	temp = 36.53+( (double)raw )/340;
	
	/* 保留俩位小数，小数转换为整数时会舍去小数部分 */
	return temp*100;
}


/**
  * @brief  ：获取陀螺仪值
  * @param  ：gx、gy、gz：是陀螺仪x、y、z轴的原始读数（16位）
  * @retval ：0：获取成功
              1：获取失败
*/
uint8_t MPU_Get_Gyroscope( short *gx, short *gy, short *gz )
{
	uint8_t buf[6],res;
	
	/* MPU_GYRO_XOUTH_REG：x轴高八位寄存器
    三个轴的寄存器地址是连续的，先高位，后低位	*/
	if( (res=MPU_Read_Continue( MPU_ADDR, MPU_GYRO_XOUTH_REG, 6, buf ))==0 )
	{
		*gx = ((uint16_t)buf[0]<<8)|buf[1];
		*gy = ((uint16_t)buf[2]<<8)|buf[3];
		*gz = ((uint16_t)buf[4]<<8)|buf[5];
	}
	
	return res;
}



/**
  * @brief  ：获取加速度计值
  * @param  ：ax、ay、az：是加速度计x、y、z轴的原始读数（16位）
  * @retval ：0：获取成功
              1：获取失败
*/
uint8_t MPU_Get_Accelerometer( short *ax, short *ay, short *az )
{
	uint8_t buf[6],res;
	
	/* MPU_ACCEL_XOUTH_REG：x轴高八位寄存器
    三个轴的寄存器地址是连续的，先高位，后低位	*/
	if( (res=MPU_Read_Continue( MPU_ADDR, MPU_ACCEL_XOUTH_REG, 6, buf ))==0 )
	{
		*ax = ((uint16_t)buf[0]<<8)|buf[1];
		*ay = ((uint16_t)buf[2]<<8)|buf[3];
		*az = ((uint16_t)buf[4]<<8)|buf[5];
	}
	
	return res;
}


1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355

4.DMP相关代码

要想使用DMP求欧拉角的代码，包含下面这几个文件即可，下面列出接口函数，到时候使用时直接使用接口函数即可。

向DMP算法提供的接口宏定义
只需要提供：对MPU6050的读写操作函数和延时函数即可

#define i2c_write   MPU_Write_Continue
#define i2c_read    MPU_Read_Continue
#define delay_ms    delay_ms
1
2
3

DMP初始化

//mpu6050,dmp初始化
//返回值:0,正常
//    其他,失败
uint8_t mpu_dmp_init(void)
{
	uint8_t 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;
}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32

获取欧拉角

//得到dmp处理后的数据(注意,本函数需要比较多堆栈,局部变量有点多)
//pitch:俯仰角 精度:0.1°   范围:-90.0° <---> +90.0°
//roll:横滚角  精度:0.1°   范围:-180.0°<---> +180.0°
//yaw:航向角   精度:0.1°   范围:-180.0°<---> +180.0°
//返回值:0,正常
//    其他,失败
uint8_t 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;
}

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38

5.mian()函数

主函数中对MPU6050进行初始化后，对DMP也进行初始化，然后就可以直接使用mpu_dmp_get_data()获取欧拉角，还可以获取温度值。

#include "stm32f10x.h"   
#include "usart.h"
#include "delay.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 

int main(void)
{
	uint8_t x=0;
	float pitch,roll,yaw; 		//欧拉角
	short aacx,aacy,aacz;		//加速度传感器原始数据
	short gyrox,gyroy,gyroz;	//陀螺仪原始数据
	short temp;					//温度
	
	NVIC_PriorityGroupConfig( 2 );
	delay_init();
	USART1_Init(115200);	
	printf("程序开始\n");
	
	if( MPU_Init()!=0 )
	{
		printf("MPU6050初始化错误！\n");
		return 0;
	}
		
	if( mpu_dmp_init() )
	{
		printf("DMP初始化错误！\n");
		return 0;
	}
	while(1)
	{
		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);	//得到陀螺仪数据
		}
		delay_ms(100);
		printf("pitch:%02f  roll:%02f  yaw:%02f\n",pitch,roll,yaw);
	}

}
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44

这就是我理解的MPU6050，后续做平衡小车的时候要读取欧拉角，先总结一下，有什么问题可以和我交流一下，大家一起进步！！！
q：2723808286