2020年02月10日 | STM32 实现 MPU6050 数据读取与倾角检测

前言

MPU6050 是一个很好玩传感器，在四轴、体感、计步等应用领域都能看到这小芯片的影子，其内部的结构、功能十分丰富，可玩度非常高。同时，对传感器采集到的数据进行分析还能得到许多信息，但此时的一些「数学小技巧」或许会让你抓狂，所以你会在网上疯狂查找资料，最终发现了本文。

然而很可惜，笔者也是一名数学渣渣（买东西找零都不会算的那种），所以这篇文章并不能教会你「卡尔曼滤波」、「协方差阵列」等连我自己都不懂的东西。事实上如果你只是想从传感器读出陀螺仪和加速度值，并简单计算一下坐标倾角（不涉及姿态解算、四元素等），其实没有想象中那么难。

PS：MPU6050 的确涉及到许多令人敬畏的数学知识，但如果只是想得到「简陋」的坐标夹角，完全没必要搬出「加速度陀螺仪融合」、「四元素欧拉角」、「陀螺仪积分」、「内部的 DMP」等名词来唬人，用高中三角函数知识已足够。当你有更高精度数据的需求时，也没必要自己从头造轮子，直接移植现成的库即可。本文将实现前一种设想 —— 使用简陋方法计算粗略角度

文章分为 3 个部分（实验）：

• 利用 I2C 协议从传感器读出 6 个数据（三轴陀螺仪 + 三轴加速度值）

• 基于 3 个加速度值，通过反三角函数计算传感器与各坐标轴夹角（倾角）

• 利用计算到倾角做个小玩意 —— 通过「重力感应（倾斜开发板）」控制流水灯的速度、方向
  

预备知识

• STM32 基本开发方法

• I2C 协议通讯基本流程原理

• 基本物理力学常识

• 简单三角函数知识
  

角速度与加速度角速度

角速度描述物体的旋转的快慢，具体指物体在单位时间内转过的角度。举个最简单的例子：钟表上的秒针 60 秒转动一圈（360 度），故秒针单位时间内转过的角度为：360 / 60 = 6 度/秒，即秒针转动的角速度。通过角速度和运动时间能计算出秒针转过角度。

在平面中，物体总是围绕着一个「旋转中心」进行旋转（如表盘的圆心），而在三维空间中，物体是围绕着一根「旋转轴」进行旋转（比如，烤羊肉串？），并且绕任何「旋转轴」的旋转都可以分解为空间直角坐标系中 X、Y、Z 三个轴上旋转的合成，这也是 MPU6050 所测量的三个角速度值，同理，得到三轴角速度分量就可以确定任一旋转状态。

加速度

加速度是指速度变化的快慢，稍有物理常识的人都知道：只有在外力的作用下，物体的速度才会变化，所以把加速度简单理解为物体受力情况似乎也没毛病。举个例子，用力把一块石头扔出去瞬间的加速度比捧着石头慢慢移动的加速度要大得多。此外，加速度同样也可以分解为三个坐标轴上的分量。

MPU6050 加速度方向

需要注意的一点是，MPU6050 读到加速度与受力方向相反。

传感器在静止状态受到向下的重力，测量到的加速度向上（了解这点很重要）。同理，如果给一个水平向左的力，测量到的加速度向右。这也是一些资料中「箱子和球」模型所表达的原理。

MPU6050 介绍

MPU-60x0 是全球首例 9 轴运动处理传感器。它集成了 3 轴MEMS陀螺仪，3 轴MEMS加速度计，以及一个可扩展的数字运动处理器 DMP（Digital Motion Processor）。 MPU-60x0 对陀螺仪和加速度计分别用了三个 16 位的 ADC，将其测量的模拟量转化为可输出的数字量。为了精确跟踪快速和慢速的运动，传感器的测量范围都是用户可控的，陀螺仪可测范围为 ±250，±500，±1000，±2000°/秒（dps），加速度计可测范围为 ±2，±4，±8，±16g。

应用范围

从上面的分析可知道，角速度能反映物体的圆周运动状态，加速度能能反映物体的静止和直线运动状态，两者互补就能识别很复杂的运动状态，如四轴飞行器的姿态、人行走的步态、空中鼠标的位移量、手机横屏竖屏等等。可以说几乎所有与运动、位置、姿态相关的应用都可以用加速度陀螺仪传感器实现。当然，从成本、开发难度考虑并不会完全这样做。

传感器坐标系

作为测量值的方向参考，传感器坐标方向定义如下图，属于右手坐标系（右手拇指指向 x 轴的正方向，食指指向 y 轴的正方向，中指能指向 z 轴的正方向）：

模块接线方法

我是使用公司 STM32F407ZGT6 开发板上集成的 MP6050 芯片，可免去接线的麻烦，如果开发板上没有相应芯片就只能使用模块了，接线如下图：

注：使用模拟 I2C 时 GPIO 不固定，图中的 PB8 和 PB9 可以根据实际情况进行更改，与代码对应即可。

I2C 基本时序函数

MPU6050 与 MCU 通过 I2C 总线进行通讯。用软件模拟的方式实现 I2C 底层基本时序函数，包括起始、停止信号的产生，以及发送/接收单字节数据、检测/发送应答信号。

// 【基础】基本数据读取USERsrci2c.h 
void I2C_Init(void); // I2C 初始化 
void I2C_Start(void); // 产生 I2C 协议起始信号 
void I2C_Stop(void); // 产生 I2C 协议结束信号 
void I2C_Write_Byte(uint8_t byte); // 发送八位数据（不包含应答） 
uint8_t I2C_Read_Byte(void); // 读取八位数据（不包含应答） 
uint8_t I2C_Read_ACK(void); // 接收应答信号 
void I2C_Write_ACK(uint8_t ack); // 发送应答信号

MCU 作为主机与传感器通讯前需要发送 7 位的从机设备地址，设备地址的惯用套路是固定高位，并通过引脚电平确定低位。查阅寄存器手册得知，117 号寄存器 WHO AM I 决定着设备地址的高 6 位，上电复位值为 110100，最低 1 位则由外部引脚 AD0 决定（即一块电路板最多只能有两个 MPU6050）。查看模块或开发板电路图确定芯片 AD0 的电平（一般为低）最终得到 7 位的 I2C 从机设备地址为 1101 000（0xD0），在 mpu6050.h 文件中宏定义为 DEV_ADDR 。

注：文章侧重讲解传感器使用，I2C 函数具体实现见文末示例代码。但在进行下面的实验之前，请确保你的 I2C 通讯是正常的、发送器件地址能得到应答。

MPU6050 基本操作函数

在实现了基本时序函数后，使用 I2C 总线跟外部设备通信实际上就是基本时序的拼接。下面编写 MPU6050 相关函数，其中读写寄存器函数是核心操作。

写寄存器写寄存器流程如下：

• 发送起始信号

• 发送设备地址（写模式）

• 发送内部寄存器地址

• 写入寄存器数据（8 位数据宽度）

• 发送结束信号
  

代码实现：/

// 【基础】基本数据读取USERsrcmpu6050.c 
/****************************************************************************** 
* 函数介绍： MPU6050 写寄存器函数 
* 输入参数： regAddr：寄存器地址 regData：待写入寄存器值 
* 输出参数： 无 
* 返回值 ： 无 
******************************************************************************/
void MPU6050_Write_Reg(uint8_t regAddr, uint8_t regData)
{
    /* 发送起始信号 */
    I2C_Start();
    
    /* 发送设备地址 */        
    I2C_Write_Byte(DEV_ADDR);
    IF (I2C_Read_ACK())
        goto stop;
    
    /* 发送寄存器地址 */
    I2C_Write_Byte(regAddr);
    if (I2C_Read_ACK())
        goto stop;
    
    /* 写数据到寄存器 */
    I2C_Write_Byte(regData);
    if (I2C_Read_ACK())
        goto stop;
    
stop:
    I2C_Stop();
}

• 发送起始信号

• 发送设备地址（写模式）

• 发送内部寄存器地址

• 发送重复起始信号

• 发送设备地址（读模式）

• 读取寄存器数据（8 位数据宽度）

• 发送结束信号

  
  

代码实现：

/ 【基础】基本数据读取USERsrcmpu6050.c 
/****************************************************************************** * 函数介绍： MPU6050 读寄存器函数 * 输入参数： regAddr：寄存器地址 * 输出参数： 无 * 返回值 ： regData：读出的寄存器数据 ******************************************************************************/
uint8_t MPU6050_Read_Reg(uint8_t regAddr)
{
    uint8_t regData;
    
    /* 发送起始信号 */
    I2C_Start();
    
    /* 发送设备地址 */        
    I2C_Write_Byte(DEV_ADDR);
    if (I2C_Read_ACK())
        goto stop;
    
    /* 发送寄存器地址 */
    I2C_Write_Byte(regAddr);
    if (I2C_Read_ACK())
        goto stop;
    
    /* 发送重复起始信号 */
    I2C_Start();
    
    /* 发送读模式设备地址 */     
    I2C_Write_Byte(DEV_ADDR | 0x01);
    if (I2C_Read_ACK())
        goto stop;
    
    /* 读寄存器数据 */
    regData = I2C_Read_Byte();
    I2C_Write_ACK(1);  // 非应答信号     
stop:
    I2C_Stop();
    
    return regData;
}

在 mpu6050.h 文件中对常用的寄存器地址进行宏定义，方便使用并提高程序可读性。

// 【基础】基本数据读取USERsrcmpu6050.h 

#define DEV_ADDR 0xD0 // 6050 器件地址 
//----------------------------------------- 
// 定义MPU6050内部地址 
//----------------------------------------- 
#define SMPLRT_DIV 0x19 //陀螺仪采样率，典型值：0x07(125Hz) 
#define CONFIG 0x1A //低通滤波频率，典型值：0x06(5Hz) 
#define GYRO_CONFIG 0x1B //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s) 
#define ACCEL_CONFIG 0x1C //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz) 

/* 加速度相关寄存器地址 */
#define ACCEL_XOUT_H 0x3B 
#define ACCEL_XOUT_L 0x3C 
#define ACCEL_YOUT_H 0x3D 
#define ACCEL_YOUT_L 0x3E 
#define ACCEL_ZOUT_H 0x3F 
#define ACCEL_ZOUT_L 0x40 

/* 温度相关寄存器地址 */
#define TEMP_OUT_H 0x41 
#define TEMP_OUT_L 0x42 

/* 陀螺仪相关寄存器地址 */
#define GYRO_XOUT_H 0x43
#define GYRO_XOUT_L 0x44 
#define GYRO_YOUT_H 0x45 
#define GYRO_YOUT_L 0x46 
#define GYRO_ZOUT_H 0x47 
#define GYRO_ZOUT_L 0x48 

#define PWR_MGMT_1 0x6B  //电源管理，典型值：0x00(正常启用) 
#define WHO_AM_I 0x75 //IIC地址寄存器(默认数值0x68，只读) 
#define SlaveAddress 0xD0 //IIC写入时的地址字节数据，+1为读取

在使用传感器测量数据之前，先要利用前面写好的读写寄存器函数，对传感器初始化，包括常用参数配置，如采样率、滤波频率等，如无特殊要求使用典型值即可（各参数具体含义请查阅芯片寄存器手册）。

代码实现：

// 【基础】基本数据读取USERsrcmpu6050.c 
/****************************************************************************** 
* 函数介绍： MPU6050 初始化函数 
* 输入参数： 无 
* 输出参数： 无 
* 返回值 ： 无 
* 备 注： 配置 MPU6050 测量范围：± 2000 °/s ± 2g 
******************************************************************************/
void MPU6050_Init(void)
{
    I2C_Init();  // I2C 初始化     

    MPU6050_Write_Reg(PWR_MGMT_1, 0x00);    //解除休眠状态     
    MPU6050_Write_Reg(SMPLRT_DIV, 0x07);    //陀螺仪采样率，典型值：0x07(125Hz)     
    MPU6050_Write_Reg(CONFIG, 0x06);        //低通滤波频率，典型值：0x06(5Hz)     
    MPU6050_Write_Reg(GYRO_CONFIG, 0x18);   //陀螺仪自检及测量范围，典型值：0x18(不自检，2000deg/s)     
    MPU6050_Write_Reg(ACCEL_CONFIG, 0x01);  //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，2G，5Hz) 
}

传感器原始数据 AD 值为 16 位数字量，从寄存器地址定义宏名可知，一个数据需要两个字节（寄存器）来表示，如 ACCEL_XOUT_H 和 ACCEL_XOUT_L 两个寄存器分别存储 X 轴加速度高 8 位和低 8 位，共同组成 16 位数据，按照这个思路，编写一个连续读两个寄存器并合成 16 位数据的函数。

代码实现：

// 【基础】基本数据读取USERsrcmpu6050.c 
/****************************************************************************** 
* 函数介绍： 连续读两个寄存器并合成 16 位数据 
* 输入参数： regAddr：数据低位寄存器地址 
* 输出参数： 无 
* 返回值 ： data：2 个寄存器合成的 16 位数据 
******************************************************************************/
int16_t MPU6050_Get_Data(uint8_t regAddr)
{
    uint8_t Data_H, Data_L;
    uint16_t data;
    
    Data_H = MPU6050_Read_Reg(regAddr);
    Data_L = MPU6050_Read_Reg(regAddr + 1);
    data = (Data_H << 8) | Data_L;  // 合成数据 
    return data;
}

写好读 16 位数据的函数，获取加速度、陀螺仪数值自然便水到渠成。只需给出待读取数据高位寄存器地址（因为高位寄存器地址在前），调用 MPU6050_Get_Data() 函数即可得到合成后 16 位加速度、温度、陀螺仪数值。如 MPU6050_Get_Data(ACCEL_XOUT_H) 表示读取 16 位 X 轴加速度数据。

在 main.c 中编写 MPU6050_display 函数，实现数据读取并在串口打印。

// 【基础】基本数据读取USERsrcmain.c 
/****************************************************************************** 
* 函数介绍： 串口打印 6050 传感器读取的三轴加速度、陀螺仪及温度数据 
* 输入参数： 无 
* 输出参数： 无 
* 返回值 ： 无 
******************************************************************************/
void MPU6050_Display(void)
{
    /* 打印 x, y, z 轴加速度 */
    printf("ACCEL_X: %dt", MPU6050_Get_Data(ACCEL_XOUT_H));
    printf("ACCEL_Y: %dt", MPU6050_Get_Data(ACCEL_YOUT_H));
    printf("ACCEL_Z: %dt", MPU6050_Get_Data(ACCEL_ZOUT_H));
    
    /* 打印温度，需要根据手册的公式换算为摄氏度 */
    printf("TEMP: %0.2ft", MPU6050_Get_Data(TEMP_OUT_H) / 340.0 + 36.53);
    
    /* 打印 x, y, z 轴角速度 */
    printf("GYRO_X: %dt", MPU6050_Get_Data(GYRO_XOUT_H));
    printf("GYRO_Y: %dt", MPU6050_Get_Data(GYRO_YOUT_H));
    printf("GYRO_Z: %dt", MPU6050_Get_Data(GYRO_ZOUT_H));
    
    printf("rn");
}

// 【基础】基本数据读取USERsrcmain.c 
/****************************************************************************** 
* 函数介绍： 主函数 
* 输入参数： 无 
* 输出参数： 无 
* 返回值 ： 无 
******************************************************************************/
int main()
{
    MPU6050_Init();
    Usrat_1_Init(84,9600,0);    
    
    while (1)
    {
        // Usart_Send_Byte(MPU6050_Read_Reg(GYRO_XOUT_L)); 
        MPU6050_Display();  // 串口打印传感器数据         
        Delay(0xfffff);
    }
}