第44章 MPU6050传感器

全套200集视频教程和1000页PDF教程请到秉火论坛下载：www.firebbs.cn

野火视频教程优酷观看网址：http://i.youku.com/firege

本章参考数据：《STM32F4xx参考手册》、《STM32F4xx规格书》、库说明文档《stm32f4xx_dsp_stdperiph_lib_um.chm》。

关于MPU6050的参考资料：《MPU-60X0寄存器》、《MPU6050数据手册》以及官方驱动《motion_driver_6.12》。

本章讲解的内容跨领域的知识较多，若您感兴趣，请自行查阅各方面的资料，对比学习。

在飞行器中，飞行姿态是非常重要的参数，见图 441，以飞机自身的中心建立坐标系，当飞机绕坐标轴旋转的时候，会分别影响偏航角、横滚角及俯仰角。

图 441 表示飞机姿态的偏航角、横滚角及俯仰角

假如我们知道飞机初始时是左上角的状态，只要想办法测量出基于原始状态的三个姿态角的变化量，再进行叠加，就可以获知它的实时姿态了。

抽象来说，姿态是"载体坐标系"与"地理坐标系"之间的转换关系。

图 442 地球坐标系、地理坐标系与载体坐标系

我们先来了解三种常用的坐标系：

    地球坐标系：以地球球心为原点，Z轴沿地球自转轴方向，X、Y轴在赤道平面内的坐标系。

    地理坐标系：它的原点在地球表面(或运载体所在的点)，Z轴沿当地地理垂线的方向(重力加速度方向)，XY轴沿当地经纬线的切线方向。根据各个轴方向的不同，可选为"东北天"、"东南天"、"西北天"等坐标系。这是我们日常生活中使用的坐标系，平时说的东南西北方向与这个坐标系东南西北的概念一致。

    载体坐标系：载体坐标系以运载体的质心为原点，一般根据运载体自身结构方向构成坐标系，如Z轴上由原点指向载体顶部，Y轴指向载体头部，X轴沿载体两侧方向。上面说基于飞机建立的坐标系就是一种载体坐标系，可类比到汽车、舰船、人体、动物或手机等各种物体。

地理坐标系与载体坐标系都以载体为原点，所以它们可以经过简单的旋转进行转换，载体的姿态角就是根据载体坐标系与地理坐标系的夹角来确定的。配合图 441，发挥您的空间想象力，假设初始状态中，飞机的Z轴、X轴及Y轴分别与地理坐标系的天轴、北轴、东轴平行。如当飞机绕自身的"Z"轴旋转，它会使自身的"Y"轴方向与地理坐标系的"南北"方向偏离一定角度，该角度就称为偏航角(Yaw)；当载体绕自身的"X"轴旋转，它会使自身的"Z"轴方向与地理坐标系的"天地"方向偏离一定角度，该角度称为俯仰角(Pitch)；当载体绕自身的"Y"轴旋转，它会使自身的"X"轴方向与地理坐标系的"东西"方向偏离一定角度，该角度称为横滚角。

表 441 姿态角的关系

坐标系间的旋转角度

说明

载体自身旋转

偏航角(Yaw)

Y轴与北轴的夹角

绕载体Z轴旋转可改变

俯仰角(Pitch)

Z轴与天轴的夹角

绕载体X轴旋转可改变

横滚角(Roll)

X轴与东轴的夹角

绕载体Y轴旋转可改变

这些角度也称欧拉角，是用于描述姿态的非常直观的角度。

最直观的角度检测器就是陀螺仪了，见图 443，它可以检测物体绕坐标轴转动的"角速度"，如同将速度对时间积分可以求出路程一样，将角速度对时间积分就可以计算出旋转的"角度"。

图 443 陀螺仪检测示意图

由于陀螺仪测量角度时使用积分，会存在积分误差，见图 444，若积分时间Dt越小，误差就越小。这十分容易理解，例如计算路程时，假设行车时间为1小时，我们随机选择行车过程某个时刻的速度Vt乘以1小时，求出的路程误差是极大的，因为行车的过程中并不是每个时刻都等于该时刻速度的，如果我们每5分钟检测一次车速，可得到Vt1、Vt2、Vt3-Vt12这12个时刻的车速，对各个时刻的速度乘以时间间隔(5分钟)，并对这12个结果求和，就可得出一个相对精确的行车路程了，不断提高采样频率，就可以使积分时间Dt变小，降低误差。

图 444 积分误差

同样地，提高陀螺仪传感器的采样频率，即可减少积分误差，目前非常普通的陀螺仪传感器的采样频率都可以达到8KHz，已能满足大部分应用的精度要求。

更难以解决的是器件本身误差带来的问题。例如，某种陀螺仪的误差是0.1度/秒，当陀螺仪静止不动时，理想的角速度应为0，无论它静止多久，对它进行积分测量得的旋转角度都是0，这是理想的状态；而由于存在0.1度/秒的误差，当陀螺仪静止不动时，它采样得的角速度一直为0.1度/秒，若静止了1分钟，对它进行积分测量得的旋转角度为6度，若静止了1小时，陀螺仪进行积分测量得的旋转角度就是360度，即转过了一整圈，这就变得无法忍受了。只有当正方向误差和负方向误差能正好互相抵消的时候，才能消除这种累计误差。

由于直接用陀螺仪测量角度在长时间测量时会产生累计误差，因而我们又引入了检测倾角的传感器。

图 445 T字型水平仪

测量倾角最常见的例子是建筑中使用的水平仪，在重力的影响下，水平仪内的气泡能大致反映水柱所在直线与重力方向的夹角关系，利用图 445中的T字型水平仪，可以检测出图 441中说明的横滚角与俯仰角，但是偏航角是无法以这样的方式检测的。

在电子设备中，一般使用加速度传感器来检测倾角，它通过检测器件在各个方向的形变情况而采样得到受力数据，根据F=ma转换，传感器直接输出加速度数据，因而被称为加速度传感器。由于地球存在重力场，所以重力在任何时刻都会作用于传感器，当传感器静止的时候(实际上加速度为0)，传感器会在该方向检测出加速度g，不能认为重力方向测出的加速度为g，就表示传感器在该方向作加速度为g的运动。

当传感器的姿态不同时，它在自身各个坐标轴检测到的重力加速度是不一样的，利用各方向的测量结果，根据力的分解原理，可求出各个坐标轴与重力之间的夹角，见图 446。

图 446 重力检测

因为重力方向是与地理坐标系的"天地"轴固连的，所以通过测量载体坐标系各轴与重力方向的夹角即可求得它与地理坐标系的角度旋转关系，从而获知载体姿态。

由于这种倾角检测方式是利用重力进行检测的，它无法检测到偏航角(Yaw)，原理跟T字型水平仪一样，无论如何设计水平仪，水泡都无法指示这样的角度。

另一个缺陷是加速度传感器并不会区分重力加速度与外力加速度，当物体运动的时候，它也会在运动的方向检测出加速度，特别在震动的状态下，传感器的数据会有非常大的数据变化，此时难以反应重力的实际值。

为了弥补加速度传感器无法检测偏航角(Yaw)的问题，我们再引入磁场检测传感器，它可以检测出各个方向上的磁场大小，通过检测地球磁场，它可实现指南针的功能，所以也被称为电子罗盘。由于地磁场与地理坐标系的"南北"轴固联，利用磁场检测传感器的指南针功能，就可以测量出偏航角(Yaw)了。

与指南针的缺陷一样，使用磁场传感器会受到外部磁场干扰，如载体本身的电磁场干扰，不同地理环境的磁铁矿干扰等等。

使用GPS可以直接检测出载体在地球上的坐标，假如载体在某时刻测得坐标为A，另一时刻测得坐标为B，利用两个坐标即可求出它的航向，即可以确定偏航角，且不受磁场的影响，但这种检测方式只有当载体产生大范围位移的时候才有效(GPS民用精度大概为10米级)。

可以发现，使用陀螺仪检测角度时，在静止状态下存在缺陷，且受时间影响，而加速度传感器检测角度时，在运动状态下存在缺陷，且不受时间影响，刚好互补。假如我们同时使用这两种传感器，并设计一个滤波算法，当物体处于静止状态时，增大加速度数据的权重，当物体处于运动状时，增大陀螺仪数据的权重，从而获得更准确的姿态数据。同理，检测偏航角，当载体在静止状态时，可增大磁场检测器数据的权重，当载体在运动状态时，增大陀螺仪和GPS检测数据的权重。这些采用多种传感器数据来检测姿态的处理算法被称为姿态融合。

在姿态融合解算的时候常常使用"四元数"来表示姿态，它由三个实数及一个虚数组成，因而被称之为四元数。使用四元数表示姿态并不直观，但因为使用欧拉角(即前面说的偏航角、横滚角及俯仰角)表示姿态的时候会有"万向节死锁"问题，且运算比较复杂，所以一般在数据处理的时候会使用四元数，处理完毕后再把四元数转换成欧拉角。在这里我们只要了解四元数是姿态的另一种表示方式即可，感兴趣的话可自行查阅相关资料。

前文提到了各种传感器，在这里大致讲解一下传感器的工作原理。我们讲的传感器一般是指把物理量转化成电信号量的装置，见图 447。

图 447传感器工作原理

敏感元件直接感受被测物理量，并输出与该物理量有确定关系的信号，经过转换元件将该物理量信号转换为电信号，变换电路对转换元件输出的电信号进行放大调制，最后输出容易检测的电信号量。例如，温度传感器可把温度量转化成电压信号量输出，且温度值与电压值成比例关系，我们只要使用ADC测量出电压值，并根据转换关系即可求得实际温度值。而前文提到的陀螺仪、加速度及磁场传感器也是类似的，它们检测的角速度、加速度及磁场强度与电压值有确定的转换关系。

传感器一般使用精度、分辨率及采样频率这些参数来进行比较，衡量它的性能，见表 442。

表 442 传感器参数

参数

说明

线性误差

指传感器测量值与真实物理量值之间的拟合度误差。

分辨率

指传感器可检测到的最小物理量的单位。

采样频率

指在单位时间内的采样次数。

其中误差与分辨率是比较容易混淆的概念，以使用尺子测量长度为例，误差就是指尺子准不准，使用它测量出10厘米，与计量机构标准的10厘米有多大区别，若区别在5毫米以内，我们则称这把尺子的误差为5毫米。而分辨率是指尺子的最小刻度值，假如尺子的最小刻度值为1厘米，我们称这把尺子的分辨率为1厘米，它只能用于测量厘米级的尺寸，对于毫米级的长度，这就无法用这把尺子进行测量了。如果把尺子加热拉长，尺子的误差会大于5毫米，但它的分辨率仍为1厘米，只是它测出的1厘米值与真实值之间差得更远了。

大部分传感器的输出都是与电压成比例关系的，电压值一般采用ADC来测量，而ADC一般有固定的位数，如8位ADC、12位ADC等，ADC的位数会影响测量的分辨率及量程。例如图 448，假设用一个2位的ADC来测量长度，2位的ADC最多只能表示0、1、2、3这四个数，假如它的分辨率为20厘米，那么它最大的测量长度为60厘米，假如它的分辨率为10厘米，那么它的最大测量长度为30厘米，由此可知，对于特定位数的ADC，量程和分辨率不可兼得。

图 448 ADC表示的物理量范围

在实际应用中，常常直接用ADC每位表征的物理量值来表示分辨率，如每位代表20厘米，我们称它的分辨率为1LSB/20cm，它等效于5位表示1米：5LSB/m。其中的LSB（Least Significant Bit），意为最ADC的低有效位。

使用采样得到的ADC数值，除以分辨率，即可求取得到物理量。例如使用分辨率为5LSB/m、线性误差为0.1m的传感器进行长度测量，其ADC采样得到数据值为"20"，可计算知道该传感器的测量值为4米，而该长度的真实值介于3.9-4.1米之间。

接下来我们使用传感器实例来讲解如何检测物体的姿态。在我们的STM32F4实验板上有一个MPU6050芯片，它是一种六轴传感器模块，采用InvenSense公司的MPU6050作为主芯片，能同时检测三轴加速度、三轴陀螺仪(三轴角速度)的运动数据以及温度数据。利用MPU6050芯片内部的DMP模块（Digital Motion Processor数字运动处理器），可对传感器数据进行滤波、融合处理，它直接通过I2C接口向主控器输出姿态解算后的姿态数据，降低主控器的运算量。其姿态解算频率最高可达200Hz，非常适合用于对姿态控制实时要求较高的领域。常见应用于手机、智能手环、四轴飞行器及计步器等的姿态检测。

图 449 MPU6050传感器的坐标及方向

图 449中表示的坐标系及旋转符号标出了MPU6050传感器的XYZ轴的加速度有角速度的正方向。

实验板中使用的MPU6050传感器参数见表 443。

表 443 MPU6050的特性参数

参数

说明

供电

3.3V-5V

通讯接口

I2C协议，支持的I2C时钟最高频率为400KHz

测量维度

加速度：3维陀螺仪：3维

ADC分辨率

加速度：16位陀螺仪：16位

加速度测量范围

±2g、±4g、±8g、±16g 其中g为重力加速度常数，g=9.8m/s ²

加速度最高分辨率

16384 LSB/g

加速度线性误差

0.1g

加速度输出频率

最高1000Hz

陀螺仪测量范围

±250 º/s 、±500 º/s 、±1000 º/s、±2000 º/s、

陀螺仪最高分辨率

131 LSB/( º/s)

陀螺仪线性误差

0.1 º/s

陀螺仪输出频率

最高 8000Hz

DMP姿态解算频率

最高200Hz

温度传感器测量范围

-40~ +85℃

温度传感器分辨率

340 LSB/℃

温度传感器线性误差

±1℃

工作温度

-40~ +85℃

功耗

500uA~3.9mA (工作电压3.3V)

该表说明，加速度与陀螺仪传感器的ADC均为16位，它们的量程及分辨率可选多种模式，见图 4411，量程越大，分辨率越低。

图 4410 加速度配置跟量程的关系

图 4411 陀螺仪的几种量程配置

从表中还可了解到传感器的加速度及陀螺仪的采样频率分别为1000Hz及8000Hz，它们是指加速度及角速度数据的采样频率，我们可以使用STM32控制器把这些数据读取出来然后进行姿态融合解算，以求出传感器当前的姿态(即求出偏航角、横滚角、俯仰角)。而如果我们使用传感器内部的DMP单元进行解算，它可以直接对采样得到的加速度及角速度进行姿态解算，解算得到的结果再输出给STM32控制器，即STM32无需自己计算，可直接获取偏航角、横滚角及俯仰角，该DMP每秒可输出200次姿态数据。

这一小节我们学习如何使用STM32控制MPU6050传感器读取加速度、角速度及温度数据。在控制传感器时，使用到了STM32的I2C驱动，就如同控制STM32一样，对MPU6050传感器的不同寄存器写入不同内容可以实现不同模式的控制，从特定的寄存器读取内容则可获取测量数据，这部分关于MPU6050具体寄存器的内容我们不再展开，请您查阅《MPU-60X0寄存器》手册获知。

STM32与MPU6050的硬件连接见图 428。

图 4412 STM32与MPU6050的硬件连接

    它的硬件连接非常简单，SDA与SCL引出到STM32的I2C引脚，注意图中的I2C没有画出上拉电阻，只是因为实验板中其它芯片也使用了同样的I2C总线，电阻画到了其它芯片的图里，没有出现在这个图中而已。传感器的I2C设备地址可通过AD0引脚的电平控制，当AD0接地时，设备地址为0x68(七位地址)，当AD0接电源时，设备地址为0x69(八位地址)。另外，传感器的INT引脚接到了STM32的普通IO口，当传感器有新数据的时候会通过INT引脚通知STM32。

    由于MPU6050检测时是基于自已中心坐标系的，所以在自己设计硬件时，您需要考虑它与所在设备的坐标系统的关系。

本小节讲解的是"MPU6050基本数据读取"实验，请打开配套的代码工程阅读理解。为了方便展示及移植，我们把STM32的I2C驱动相关的代码都编写到"i2c.c"及"i2c.h"文件中，与MPU6050传感器相关的代码都写到"mpu6050.c"及"mpu6050.h"文件中，这些文件是我们自己编写的，不属于标准库的内容，可根据您的喜好命名文件。

(4)    初始化STM32的I2C；

(5)    使用I2C向MPU6050写入控制参数；

(6)    定时读取加速度、角速度及温度数据。

本实验中的I2C驱动与MPU6050驱动分开主要是考虑到扩展其它传感器时的通用性，如使用磁场传感器、气压传感器都可以使用同样一个I2C驱动，这个驱动只要给出针对不同传感器时的不同读写接口即可。关于STM32的I2C驱动原理请参考读写EEPROM的章节，本章讲解的I2C驱动主要针对接口封装讲解，细节不再赘述。本实验中的I2C硬件定义见代码清单 441。

代码清单 441 I2C的硬件定义(i2c.h文件)

1 /*引脚定义*/

2

3 #define SENSORS_I2C_SCL_GPIO_PORT GPIOB

4 #define SENSORS_I2C_SCL_GPIO_CLK RCC_AHB1Periph_GPIOB

5 #define SENSORS_I2C_SCL_GPIO_PIN GPIO_Pin_6

6 #define SENSORS_I2C_SCL_GPIO_PINSOURCE GPIO_PinSource6

7

8 #define SENSORS_I2C_SDA_GPIO_PORT GPIOB

9 #define SENSORS_I2C_SDA_GPIO_CLK RCC_AHB1Periph_GPIOB

10 #define SENSORS_I2C_SDA_GPIO_PIN GPIO_Pin_7

11 #define SENSORS_I2C_SDA_GPIO_PINSOURCE GPIO_PinSource7

12

13 #define SENSORS_I2C_AF GPIO_AF_I2C1

14

15 #define SENSORS_I2C I2C1

16 #define SENSORS_I2C_RCC_CLK RCC_APB1Periph_I2C1

    这些宏根据传感器使用的I2C硬件封装起来了。

接下来利用这些宏对I2C进行初始化，初始化过程与I2C读写EEPROM中的无异，见代码清单 442。

代码清单 442 初始化I2C（i2c.c文件）

1 /**

2 * @brief 初始化I2C总线，使用I2C前需要调用

3 * @param 无

4 * @retval 无

5 */

6 void I2cMaster_Init(void)

7 {

8 GPIO_InitTypeDef GPIO_InitStructure;

9 I2C_InitTypeDef I2C_InitStructure;

10

11 /* Enable I2Cx clock */

12 RCC_APB1PeriphClockCmd(SENSORS_I2C_RCC_CLK, ENABLE);

13

14 /* Enable I2C GPIO clock */

15 RCC_AHB1PeriphClockCmd(SENSORS_I2C_SCL_GPIO_CLK |

16 SENSORS_I2C_SDA_GPIO_CLK, ENABLE);

17

18 /* Configure I2Cx pin: SCL ----------------------------------------*/

19 GPIO_InitStructure.GPIO_Pin = SENSORS_I2C_SCL_GPIO_PIN;

20 GPIO_InitStructure.GPIO_Mode = GPIO_Mode_AF;

21 GPIO_InitStructure.GPIO_Speed = GPIO_Speed_100MHz;

22 GPIO_InitStructure.GPIO_OType = GPIO_OType_OD;

23 GPIO_InitStructure.GPIO_PuPd = GPIO_PuPd_NOPULL;

24

25 /* Connect pins to Periph */

26 GPIO_PinAFConfig(SENSORS_I2C_SCL_GPIO_PORT, SENSORS_I2C_SCL_GPIO_PINSOURCE,

27 SENSORS_I2C_AF);

28 GPIO_Init(SENSORS_I2C_SCL_GPIO_PORT, &GPIO_InitStructure);

29

30 /* Configure I2Cx pin: SDA ----------------------------------------*/

31 GPIO_InitStructure.GPIO_Pin = SENSORS_I2C_SDA_GPIO_PIN;

32

33 /* Connect pins to Periph */

34 GPIO_PinAFConfig(SENSORS_I2C_SDA_GPIO_PORT, SENSORS_I2C_SDA_GPIO_PINSOURCE,

35 SENSORS_I2C_AF);

36 GPIO_Init(SENSORS_I2C_SDA_GPIO_PORT, &GPIO_InitStructure);

37

38 I2C_DeInit(SENSORS_I2C);

39 I2C_InitStructure.I2C_Mode = I2C_Mode_I2C;

40 I2C_InitStructure.I2C_DutyCycle = I2C_DutyCycle_2;

41 I2C_InitStructure.I2C_OwnAddress1 = I2C_OWN_ADDRESS;

42 I2C_InitStructure.I2C_Ack = I2C_Ack_Enable;

43 I2C_InitStructure.I2C_AcknowledgedAddress = I2C_AcknowledgedAddress_7bit;

44 I2C_InitStructure.I2C_ClockSpeed = I2C_SPEED;

45

46 /* Enable the I2C peripheral */

47 I2C_Cmd(SENSORS_I2C, ENABLE);

48

49 /* Initialize the I2C peripheral */

50 I2C_Init(SENSORS_I2C, &I2C_InitStructure);

51

52 return;

53 }

初始化完成后就是编写I2C读写函数了，这部分跟EERPOM的一样，主要是调用STM32标准库函数读写数据寄存器及标志位，本实验的这部分被编写进ST_Sensors_I2C_WriteRegister及ST_Sensors_I2C_ReadRegister中了，在它们之上，再封装成了Sensors_I2C_WriteRegister及Sensors_I2C_ReadRegister，见代码清单 443。

代码清单 443 对读写函数的封装(i2c.c文件)

1

2 /**

3 * @brief 写寄存器(多次尝试)，这是提供给上层的接口

4 * @param slave_addr: 从机地址

5 * @param reg_addr:寄存器地址

6 * @param len：写入的长度

7 * @param data_ptr:指向要写入的数据

8 * @retval 正常为0，不正常为非0

9 */

10 int Sensors_I2C_WriteRegister(unsigned char slave_addr,

11 unsigned char reg_addr,

12 unsigned short len,

13 const unsigned char *data_ptr)

14 {

15 char retries=0;

16 int ret = 0;

17 unsigned short retry_in_mlsec = Get_I2C_Retry();

18

19 tryWriteAgain:

20 ret = 0;

21 ret = ST_Sensors_I2C_WriteRegister( slave_addr, reg_addr, len, data_ptr);

22

23 if (ret && retry_in_mlsec) {

24 if ( retries++ > 4 )

25 return ret;

26

27 Delay(retry_in_mlsec);

28 goto tryWriteAgain;

29 }

30 return ret;

31 }

32

33 /**

34 * @brief 读寄存器(多次尝试)，这是提供给上层的接口

35 * @param slave_addr: 从机地址

36 * @param reg_addr:寄存器地址

37 * @param len：要读取的长度

38 * @param data_ptr:指向要存储数据的指针

39 * @retval 正常为0，不正常为非0

40 */

41 int Sensors_I2C_ReadRegister(unsigned char slave_addr,

42 unsigned char reg_addr,

43 unsigned short len,

44 unsigned char *data_ptr)

45 {

46 char retries=0;

47 int ret = 0;

48 unsigned short retry_in_mlsec = Get_I2C_Retry();

49

50 tryReadAgain:

51 ret = 0;

52 ret = ST_Sensors_I2C_ReadRegister( slave_addr, reg_addr, len, data_ptr);

53

54 if (ret && retry_in_mlsec) {

55 if ( retries++ > 4 )

56 return ret;

57

58 Delay(retry_in_mlsec);

59 goto tryReadAgain;

60 }

61 return ret;

62 }

封装后的函数主要是增加了错误重试机制，若读写出现错误，则会进行多次尝试，多次尝试均失败后会返回错误代码。这个函数作为I2C驱动对外的接口，其它使用I2C的传感器调用这个函数进行读写寄存器。

MPU6050有各种各样的寄存器用于控制工作模式，我们把这些寄存器的地址、寄存器位使用宏定义到了mpu6050.h文件中了，见代码清单 444。

代码清单 444MPU6050的寄存器定义(mpu6050.h)

1 // MPU6050, Standard address 0x68

2 #define MPU6050_ADDRESS 0x68

3 #define MPU6050_WHO_AM_I 0x75

4 #define MPU6050_SMPLRT_DIV 0 //8000Hz

5 #define MPU6050_DLPF_CFG 0

6 #define MPU6050_GYRO_OUT 0x43 //MPU6050陀螺仪数据寄存器地址

7 #define MPU6050_ACC_OUT 0x3B //MPU6050加速度数据寄存器地址

8

9 #define MPU6050_ADDRESS_AD0_LOW 0x68 //AD0为低电平时的地址

10 #define MPU6050_ADDRESS_AD0_HIGH 0x69 //AD0为高电平时的地址

11 #define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW

12

13 #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD

14 #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD

15 #define MPU6050_RA_ZG_OFFS_TC 0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD

16 #define MPU6050_RA_X_FINE_GAIN 0x03 //[7:0] X_FINE_GAIN

17 /*.........以下部分省略*/

根据MPU6050的寄存器功能定义，我们使用I2C往寄存器写入特定的控制参数，见代码清单 445。

代码清单 445 初始化MPU6050

1

2 /**

3 * @brief 写数据到MPU6050寄存器

4 * @param reg_add:寄存器地址

5 * @param reg_data:要写入的数据

6 * @retval

7 */

8 void MPU6050_WriteReg(u8 reg_add,u8 reg_dat)

9 {

10 Sensors_I2C_WriteRegister(MPU6050_ADDRESS,reg_add,1,®_dat);

11 }

12

13 /**

14 * @brief 从MPU6050寄存器读取数据

15 * @param reg_add:寄存器地址

16 * @param Read：存储数据的缓冲区

17 * @param num：要读取的数据量

18 * @retval

19 */

20 void MPU6050_ReadData(u8 reg_add,unsigned char* Read,u8 num)

21 {

22 Sensors_I2C_ReadRegister(MPU6050_ADDRESS,reg_add,num,Read);

23 }

24

25

26 /**

27 * @brief 初始化MPU6050芯片

28 * @param

29 * @retval

30 */

31 void MPU6050_Init(void)

32 {

33 int i=0,j=0;

34 //在初始化之前要延时一段时间，若没有延时，则断电后再上电数据可能会出错

35 for (i=0; i> 24);

24 out[4] = (char)(quat[0] >> 16);

25 out[5] = (char)(quat[0] >> 8);

26 out[6] = (char)quat[0];

27 out[7] = (char)(quat[1] >> 24);

28 out[8] = (char)(quat[1] >> 16);

29 out[9] = (char)(quat[1] >> 8);

30 out[10] = (char)quat[1];

31 out[11] = (char)(quat[2] >> 24);

32 out[12] = (char)(quat[2] >> 16);

33 out[13] = (char)(quat[2] >> 8);

34 out[14] = (char)quat[2];

35 out[15] = (char)(quat[3] >> 24);

36 out[16] = (char)(quat[3] >> 16);

37 out[17] = (char)(quat[3] >> 8);

38 out[18] = (char)quat[3];

39 out[21] = '\r';

40 out[22] = '\n';

41

42 for (i=0; i hal.next_temp_ms) {

177 hal.next_temp_ms = timestamp + TEMP_READ_MS;

178 new_temp = 1;

179 }

180

181 if (hal.new_gyro && hal.dmp_on) {

182 short gyro[3], accel_short[3], sensors;

183 unsigned char more;

184 long accel[3], quat[4], temperature;

185 /* This function gets new data from the FIFO when the DMP is in

186 * use. The FIFO can contain any combination of gyro, accel,

187 * quaternion, and gesture data. The sensors parameter tells the

188 * caller which data fields were actually populated with new data.

189 * For example, if sensors == (INV_XYZ_GYRO | INV_WXYZ_QUAT), then

190 * the FIFO isn't being filled with accel data.

191 * The driver parses the gesture data to determine if a gesture

192 * event has occurred; on an event, the application will be notified

193 * via a callback (assuming that a callback function was properly

194 * registered). The more parameter is non-zero if there are

195 * leftover packets in the FIFO.

196 */

197 dmp_read_fifo(gyro, accel_short, quat, &sensor_timestamp, &sensors, &more);

198 if (!more)

199 hal.new_gyro = 0;

200 if (sensors & INV_XYZ_GYRO) {

201 /* Push the new data to the MPL. */

202 inv_build_gyro(gyro, sensor_timestamp);

203 new_data = 1;

204 if (new_temp) {

205 new_temp = 0;

206 /* Temperature only used for gyro temp comp. */

207 mpu_get_temperature(&temperature, &sensor_timestamp);

208 inv_build_temp(temperature, sensor_timestamp);

209 }

210 }

211 if (sensors & INV_XYZ_ACCEL) {

212 accel[0] = (long)accel_short[0];

213 accel[1] = (long)accel_short[1];

214 accel[2] = (long)accel_short[2];

215 inv_build_accel(accel, 0, sensor_timestamp);

216 new_data = 1;

217 }

218 if (sensors & INV_WXYZ_QUAT) {

219 inv_build_quat(quat, 0, sensor_timestamp);

220 new_data = 1;

221 }

222 }

223

224 if (new_data) {

225 inv_execute_on_data();

226 /* This function reads bias-compensated sensor data and sensor

227 * fusion outputs from the MPL. The outputs are formatted as seen

228 * in eMPL_outputs.c. This function only needs to be called at the

229 * rate requested by the host.

230 */

231 read_from_mpl();

232 }

233 }

234 }

如您所见，main函数非常长，而且我们只是摘抄了部分，在原工程代码中还有很多代码，例如加入磁场数据使用9轴数据进行解算的功能(这是MPU9150的功能，MPU6050不支持)以及其它工作模式相关的控制示例。上述main函数的主要执行流程概括如下：

(1)    初始化STM32的硬件，如Systick、LED、调试串口、INT中断引脚以及I2C外设的初始化；

(2)    调用MPL库函数mpu_init初始化传感器的基本工作模式(以下过程调用的大部分都是MPL库函数，不再强调)；

(3)    调用inv_init_mpl函数初始化MPL软件库，初始化后才能正常进行解算；

(4)    设置各种运算参数，如四元数运算(inv_enable_quaternion)、6轴或9轴数据融合(inv_enable_9x_sensor_fusion)等等；

(5)    设置传感器的工作模式(mpu_set_sensors)、采样率(mpu_set_sample_rate)、分辨率(inv_set_gyro_orientation_and_scale)等等；

(6)    当STM32驱动、MPL库、传感器工作模式、DMP工作模式等所有初始化工作都完成后进行while循环；

(7)    在while循环中检测串口的输入，若串口有输入，则调用handle_input根据串口输入的字符(命令)，切换工作方式。这部分主要是为了支持上位机通过输入命令，根据进行不同的处理，如开、关加速度信息的采集或调试信息的输出等；

(8)    在while循环中检测是否有数据更新(if (hal.new_gyro && hal.dmp_on))，当有数据更新的时候产生INT中断，会使hal.new_gyro置1的，从而执行if里的条件代码；

(9)    使用dmp_read_fifo把数据读取到FIFO，这个FIFO是指MPL软件库定义的一个缓冲区，用来缓冲最新采集得的数据；

(10)    调用inv_build_gyro、inv_build_temp、inv_build_accel及inv_build_quat函数处理数据角速度、温度、加速度及四元数数据，并对标志变量new_data置1；

(11)    在while循环中检测new_data标志位，当有新的数据时执行if里的条件代码；

(12)    调用inv_execute_on_data函数更新所有数据及状态；

(13)    调用read_from_mpl函数向输出最新的数据。

在上面main中最后调用的read_from_mpl函数演示了如何调用MPL数据输出接口，通过这些接口我们可以获得想要的数据，其函数定义见代码清单 4417。

代码清单 4417 MPL的数据输出接口(main.c)

1 /* Get data from MPL.

2 * TODO: Add return values to the inv_get_sensor_type_xxx APIs to differentiate

3 * between new and stale data.

4 */

5 static void read_from_mpl(void)

6 {

7 long msg, data[9];

8 int8_t accuracy;

9 unsigned long timestamp;

10 float float_data[3] = {0};

11

12 if (inv_get_sensor_type_quat(data, &accuracy, (inv_time_t*)×tamp)) {

13 /* Sends a quaternion packet to the PC. Since this is used by the Python

14 * test app to visually represent a 3D quaternion, it's sent each time

15 * the MPL has new data.

16 */

17 eMPL_send_quat(data);

18

19 /* Specific data packets can be sent or suppressed using USB commands. */

20 if (hal.report & PRINT_QUAT)

21 eMPL_send_data(PACKET_DATA_QUAT, data);

22 }

23

24 if (hal.report & PRINT_ACCEL) {

25 if (inv_get_sensor_type_accel(data, &accuracy,

26 (inv_time_t*)×tamp))

27 eMPL_send_data(PACKET_DATA_ACCEL, data);

28 }

29 if (hal.report & PRINT_GYRO) {

30 if (inv_get_sensor_type_gyro(data, &accuracy,

31 (inv_time_t*)×tamp))

32 eMPL_send_data(PACKET_DATA_GYRO, data);

33 }

34

35 if (hal.report & PRINT_EULER) {

36 if (inv_get_sensor_type_euler(data, &accuracy,

37 (inv_time_t*)×tamp))

38 eMPL_send_data(PACKET_DATA_EULER, data);

39 }

40

41

42 /********************使用液晶屏显示数据**************************/

43 if (1) {

44 char cStr [ 70 ];

45 unsigned long timestamp,step_count,walk_time;

46

47

48 /*获取欧拉角*/

49 if (inv_get_sensor_type_euler(data, &accuracy,(inv_time_t*)×tamp)) {

50

51 #ifdef USE_LCD_DISPLAY

52 //inv_get_sensor_type_euler读出的数据是Q16格式，所以左移16位.

53 sprintf ( cStr, "Pitch : %.4f ", data[0]*1.0/(1