【嵌入式模块】MPU6050

0 前言

  作为惯性传感器中入门级别的器件，MPU6050凭借它出色的性价比成为一款非常常用的角度姿态传感器，在很多科创项目中被使用。我之前也接触过很多次这个器件，也收集了不少资料，趁此机会总结一下学习笔记。

1 MPU6050概述

1.1 基本概述

  MPU6050包含3轴陀螺仪和3轴加速度计，其中陀螺仪的主要作用是测量物体绕芯片的三个坐标轴的角速度，其原理是高速旋转的转子指向的方向会保持不变，即所谓陀螺效应，详细介绍建议自行搜索“陀螺仪工作原理”；加速度计则是测量三个轴向的加速度，同样也是指芯片的三个坐标轴。其原理可以想象成一个立方体箱子里面有一个失重悬空的小球，当受到外界压力时，小球会朝着某个方向运动，箱子内壁就会受到对应大小的压力，从而可以计算出各个方向的加速度大小。

  当整体受到向左的加速度时，小球会有一个相对箱子向右的加速度，从而右侧“箱壁”会受到对应加速度大小的力，这样就能计算出加速度的大小。

  MPU6050是InvenSense公司推出的全球首款整合性6轴运动处理组件。目前InvenSense已被日本的TDK公司收购，在他官网（https://invensense.tdk.com/），可能是年代久远，MPU6050已经是该公司的边缘产品了，6轴芯片当中，6050和6500两款芯片被排在最后，还都是NOT RECOMMENDED的状态，而且资料支持也不是很完善，找遍了网站，也只找到了一个放数据手册的网页，开发相关的寄存器手册并未找到。因此，建议还是在网上去搜索资料吧，如下图所示，都是网上流传的经典资料。

  MPU6050的核心就是它内部的寄存器。它内部有118个寄存器（编号从0到117）。其中需要注意，虽然寄存器手册上寄存器编号是从13开始，但实际上13之前的寄存器也是可以使用的（看后面的代码就知道了）。
  这些寄存器有一些是用来设置参数的，可读可写；也有一些是存放一些数据供外部读取的，只可读。它是基于IIC进行通信，因此，在使用时，先找到传输器件地址，然后再传输寄存器地址，最后传输相应的数据或者指令，这也是IIC协议和器件寄存器交互的常用设定。

  既然有这么多寄存器，那用起来岂不是很困难？并不是，虽然寄存器多，但实际使用时也不需要使用如此多的寄存器。而且即使要使用的话，也可以利用C语言中的宏定义，这样也不麻烦。

1.2 引脚和常用原理图

  为了将这个芯片集成到我们需要的系统当中，就需要了解这款芯片的引脚和它常用的原理图，如下图所示，这个是市面上卖的MPU6050模块的原理图。

可以看到，这里引出了8个引脚，分别是电源引脚5V和GND，IIC通信引脚SCL和SDA，一般来说，大部分的应用只需要接这四个引脚即可。其中，XCL和XDA是额外的IIC通信引脚，主要用于连接外部的磁力传感器，并利用自带的运动处理器DMP硬件加速引擎，通过主IIC接口，向应用输出完整的9轴融合演算数据。
  而AD0引脚是用来设置IIC通信中的从机地址，如果接地（不接），则从机地址为0x68， 如果接高电平，则从机地址为0x69。而INT引脚主要用于中断，如果要使用中断需要设置相关的寄存器。

2 代码

  了解了MPU6050的基础知识，接下来就是写代码来使用了。如果还没确定使用的微处理器，我推荐先使用Arduino，因为它集成了很多的第三方库，这样在使用一些器件时不用自己再重复造轮子，只需要会调用即可。
  在Arduino中也有MPU6050的库，如下图所示。

安装好库之后，接下来就找到给出的例子来学习它内部的代码了。

这里提供了6个例子，基本包含了大部分的使用。

  当然，使用Arduino IDE也存在一个问题，那就是代码不能定位过去，查看库的源码不太方便，因此建议自己基于VS Code配一个Arduino的环境，或者直接下载插件Platform IO这个插件，具体的教程建议自行搜索。
  具体可以看一下这个库的源码，主要是以下几个文件，各自的作用已标注清楚。

因此，如果不需要使用DMP时，只需要包含"MPU6050.h"即可。

  那如果是其他的微处理器呢？比如51或者STM32等。这个可以考虑在网上找一些现成的，也可以考虑自己根据这个库文件的源码自己写一个适配某个处理器的库。本质就是IIC通信和寄存器的读取。这里放一个基于51的网上流传甚广的代码。

//****************************************
// Update to MPU6050 by shinetop
// MCU: STC89C52
// 2012.3.1
// 功能: 显示加速度计和陀螺仪的10位原始数据
//****************************************
// 使用单片机STC89C52
// 晶振：11.0592M
// 显示：串口
// 编译环境 Keil uVision2
//****************************************
#include <REG52.H>
#include <math.h>    //Keil library  
#include <stdio.h>   //Keil library	
#include <INTRINS.H> //Keil library	
typedef unsigned char  uchar;
typedef unsigned short ushort;
typedef unsigned int   uint;
//****************************************
// 定义51单片机端口
//****************************************
sbit    SCL = P1 ^ 5;			//IIC时钟引脚定义
sbit    SDA = P1 ^ 4;			//IIC数据引脚定义
//****************************************
// 定义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为读取
//**************************************************************************************************
//定义类型及变量
//**************************************************************************************************
uchar dis[6];					//显示数字(-511至512)的字符数组
int	dis_data;					//变量
//**************************************************************************************************
//函数声明
//**************************************************************************************************
void  Delay5us();
void  delay(unsigned int k);										//延时
void  lcd_printf(uchar* s, int temp_data);
//********************************MPU6050操作函数***************************************************
void  InitMPU6050();											//初始化MPU6050
void  I2C_Start();
void  I2C_Stop();
void  I2C_SendACK(bit ack);
bit   I2C_RecvACK();
void  I2C_SendByte(uchar dat);
uchar I2C_RecvByte();
void  I2C_ReadPage();
void  I2C_WritePage();

void  display_ACCEL_x();
void  display_ACCEL_y();
void  display_ACCEL_z();
uchar Single_ReadI2C(uchar REG_Address);						//读取I2C数据
void  Single_WriteI2C(uchar REG_Address, uchar REG_data);	   //向I2C写入数据
//********************************************************************************
//整数转字符串
//********************************************************************************
void lcd_printf(uchar* s, int temp_data)
{
    
    
    if(temp_data < 0)
    {
    
    
        temp_data = -temp_data;
        *s = '-';
    }
    else *s = ' ';

    *++s = temp_data / 10000 + 0x30;
    temp_data = temp_data % 10000; //取余运算

    *++s = temp_data / 1000 + 0x30;
    temp_data = temp_data % 1000; //取余运算

    *++s = temp_data / 100 + 0x30;
    temp_data = temp_data % 100; //取余运算
    *++s = temp_data / 10 + 0x30;
    temp_data = temp_data % 10;  //取余运算
    *++s = temp_data + 0x30;
}
//******************************************************************************************************
//串口初始化
//*******************************************************************************************************
void init_uart()
{
    
    
    TMOD = 0x21;
    TH1 = 0xfd;		//实现波特率9600（系统时钟11.0592MHZ）
    TL1 = 0xfd;

    SCON = 0x50;
    PS = 1;    //串口中断设为高优先级别
    TR0 = 1;	 //启动定时器
    TR1 = 1;
    ET0 = 1;   //打开定时器0中断
    ES = 1;
    EA = 1;
}
//*************************************************************************************************
//串口发送函数
//*************************************************************************************************
void  SeriPushSend(uchar send_data)
{
    
    
    SBUF = send_data;
    while(!TI);
    TI = 0;
}
//*************************************************************************************************
//************************************延时*********************************************************
//*************************************************************************************************
void delay(unsigned int k)
{
    
    
    unsigned int i, j;
    for(i = 0; i < k; i++)
    {
    
    
        for(j = 0; j < 121; j++);
    }
}
//************************************************************************************************
//延时5微秒(STC90C52RC@12M)
//不同的工作环境,需要调整此函数
//注意当改用1T的MCU时,请调整此延时函数
//************************************************************************************************
void Delay5us()
{
    
    
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
    _nop_();
}
//*************************************************************************************************
//I2C起始信号
//*************************************************************************************************
void I2C_Start()
{
    
    
    SDA = 1;                    //拉高数据线
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    SDA = 0;                    //产生下降沿
    Delay5us();                 //延时
    SCL = 0;                    //拉低时钟线
}
//*************************************************************************************************
//I2C停止信号
//*************************************************************************************************
void I2C_Stop()
{
    
    
    SDA = 0;                    //拉低数据线
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    SDA = 1;                    //产生上升沿
    Delay5us();                 //延时
}
//**************************************************************************************************
//I2C发送应答信号
//入口参数:ack (0:ACK 1:NAK)
//**************************************************************************************************
void I2C_SendACK(bit ack)
{
    
    
    SDA = ack;                  //写应答信号
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    SCL = 0;                    //拉低时钟线
    Delay5us();                 //延时
}
//****************************************************************************************************
//I2C接收应答信号
//****************************************************************************************************
bit I2C_RecvACK()
{
    
    
    SCL = 1;                    //拉高时钟线
    Delay5us();                 //延时
    CY = SDA;                   //读应答信号
    SCL = 0;                    //拉低时钟线
    Delay5us();                 //延时
    return CY;
}
//*****************************************************************************************************
//向I2C总线发送一个字节数据
//*****************************************************************************************************
void I2C_SendByte(uchar dat)
{
    
    
    uchar i;
    for(i = 0; i < 8; i++)      //8位计数器
    {
    
    
        dat <<= 1;              //移出数据的最高位
        SDA = CY;               //送数据口
        SCL = 1;                //拉高时钟线
        Delay5us();             //延时
        SCL = 0;                //拉低时钟线
        Delay5us();             //延时
    }
    I2C_RecvACK();
}
//*****************************************************************************************************
//从I2C总线接收一个字节数据
//******************************************************************************************************
uchar I2C_RecvByte()
{
    
    
    uchar i;
    uchar dat = 0;
    SDA = 1;                    //使能内部上拉,准备读取数据,
    for(i = 0; i < 8; i++)      //8位计数器
    {
    
    
        dat <<= 1;
        SCL = 1;                //拉高时钟线
        Delay5us();             //延时
        dat |= SDA;             //读数据
        SCL = 0;                //拉低时钟线
        Delay5us();             //延时
    }
    return dat;
}
//*****************************************************************************************************
//向I2C设备写入一个字节数据
//*****************************************************************************************************
void Single_WriteI2C(uchar REG_Address, uchar REG_data)
{
    
    
    I2C_Start();                  //起始信号
    I2C_SendByte(SlaveAddress);   //发送设备地址+写信号
    I2C_SendByte(REG_Address);    //内部寄存器地址，
    I2C_SendByte(REG_data);       //内部寄存器数据，
    I2C_Stop();                   //发送停止信号
}
//*******************************************************************************************************
//从I2C设备读取一个字节数据
//*******************************************************************************************************
uchar Single_ReadI2C(uchar REG_Address)
{
    
    
    uchar REG_data;
    I2C_Start();                   //起始信号
    I2C_SendByte(SlaveAddress);    //发送设备地址+写信号
    I2C_SendByte(REG_Address);     //发送存储单元地址，从0开始
    I2C_Start();                   //起始信号
    I2C_SendByte(SlaveAddress + 1);//发送设备地址+读信号
    REG_data = I2C_RecvByte();     //读出寄存器数据
    I2C_SendACK(1);                //接收应答信号
    I2C_Stop();                    //停止信号
    return REG_data;
}
//******************************************************************************************************
//初始化MPU6050
//******************************************************************************************************
void InitMPU6050()
{
    
    
    Single_WriteI2C(PWR_MGMT_1, 0x00);	//解除休眠状态
    Single_WriteI2C(SMPLRT_DIV, 0x07);
    Single_WriteI2C(CONFIG, 0x06);
    Single_WriteI2C(GYRO_CONFIG, 0x18);
    Single_WriteI2C(ACCEL_CONFIG, 0x01);
}
//******************************************************************************************************
//合成数据
//******************************************************************************************************
int GetData(uchar REG_Address)
{
    
    
    uchar H, L;
    H = Single_ReadI2C(REG_Address);
    L = Single_ReadI2C(REG_Address + 1);
    return ((H << 8) + L); //合成数据
}
//******************************************************************************************************
//超级终端（串口调试助手）上显示10位数据
//******************************************************************************************************
void Display10BitData(int value)
{
    
    
    uchar i;
//	value/=64;							//转换为10位数据
    lcd_printf(dis, value);			//转换数据显示
    for(i = 0; i < 6; i++)
    {
    
    
        SeriPushSend(dis[i]);
    }

    // 	DisplayListChar(x,y,dis,4);	//启始列，行，显示数组，显示长度
}
//*******************************************************************************************************
//主程序
//*******************************************************************************************************
void main()
{
    
    
    delay(500);		//上电延时
    init_uart();
    InitMPU6050();	//初始化MPU6050
    delay(150);
    while(1)
    {
    
    
        Display10BitData(GetData(ACCEL_XOUT_H));	//显示X轴加速度
        Display10BitData(GetData(ACCEL_YOUT_H));	//显示Y轴加速度
        Display10BitData(GetData(ACCEL_ZOUT_H));	//显示Z轴加速度
        Display10BitData(GetData(GYRO_XOUT_H));		//显示X轴角速度
        Display10BitData(GetData(GYRO_YOUT_H));		//显示Y轴角速度
        Display10BitData(GetData(GYRO_ZOUT_H));		//显示Z轴角速度

        SeriPushSend(0x0d);
        SeriPushSend(0x0a);//换行，回车
        delay(2000);
    }
}

3 姿态解算

  前面提到MPU6050有很多的寄存器，但其实最核心的就是陀螺仪和加速度读取的数值，即姿态数据。但是，需要注意的是，前面也强调过，这个读取到的姿态数据是基于元器件坐标系的，而在实际应用中需要的更多是相对于大地坐标系的数据。这样得到的才是真正意义上的姿态。

  首先要搞清楚芯片坐标系的样式，这直接关系到所测数据的正方向。如下图所示。

芯片水平放置，丝印朝正上方，此时，前方是+Y方向，右侧是+X方向，正上方是+Z方向，三轴符合右手坐标系。至于旋转的角速度的正方向，也是按照右手定则，大拇指指向轴的正方向，四指所指的方向为旋转的正方向。

  关于MPU6050的姿态解算，主要有两种方式，分别是基于欧拉角和旋转矩阵推导 和直接调用芯片中的DMP模块，这里简要介绍一下。

3.1 欧拉角&旋转矩阵

  关于欧拉角和旋转矩阵，如果不理解基础知识的建议翻阅我之前的一篇博客：

【学习笔记】空间坐标系旋转与四元数

了解基本的旋转矩阵的知识后，接下来就是利用旋转矩阵来计算芯片的姿态角了。这里建议参考这篇文章，写得比较详细。

有一点存在一点疑问，那就是文章中提到的是Z-Y-X欧拉角，但我认为应该叫Z-Y-X固定角更合理，这也符合教材上的矩阵相乘的顺序。

此外，这篇文章当中陀螺仪和加速度计解算是分开的，加速度主要负责静态的姿态角，陀螺仪反应动态的姿态角变化，然后设定不同的权值，加权求和得到。其实数据融合更加合理的应该是使用卡尔曼滤波。这个后续也会跟进补充。

3.2 DMP

  DMP（Digital Motion Processor），即数字运动处理器，是MPU6050芯片内置的一个重要模块。它的作用就是根据测得的陀螺仪和加速度数据计算得到芯片的欧拉角yaw，pitch，roll的值。使用者可以不用关心它内部是怎么实现的。如果想了解的话可以去找找DMP相关的资料。

3 校正

  IMU有一个重要的特性，那就是它的误差是会随时间累积的，最好是隔一段时间校正一次。而所谓校正，就一定要有一个参考对象。对于MPU6050来说，一般参考对象就是地面。

  校正时，首先将MPU6050放置水平，保持静止。然后运行代码，读取陀螺仪和加速度数据。理论上来说，水平静止放置，陀螺仪数值应该为零，不为零则是误差，将误差写入到芯片内部“偏差寄存器”中，再次读取数值，计算误差，如此反复，直到误差值在允许范围之类，记下误差值，写入到偏差寄存器中。
  至于加速度数值，要考虑重力的影响，因为是水平放置，所以重力只在Z轴有分量。即Z轴分量为g，而默认加速度的单位是2g，而加速度数值用16位寄存器表示，且第一位为符号位（有正负之分），因此实际数值为$\frac{\left(2^{15}-1\right)}{2}=16383.5\approx 16384$，但由于这个重力加速度与Z轴正方向相反，故值为负的，即-16384.
  当然，有时候因为安装等原因，IMU校正时不能Z轴保持竖直，比如X轴保持竖直，则将实际数值代入到校正函数中，没啥差别。这里有一篇博客给出了一个测试代码，建议仔细阅读。
  关于校正，实际上也可以看作是一个自动控制系统，因此也可以采用如PID或者是机器学习等控制方法来进行校正。
  关于PID的例子，其实上面提到的Arduino的库中就有使用，即它内部的calibration()函数，具体原理可以去找源码查看。
  关于机器学习的例子，是我找到的一篇博客，用的是梯度下降的方式来取值，思路很清奇，但个人感觉本质上还是迭代，是否使用梯度下降差别不是很大。