深入学习MPU6050

自初次接触这个芯片已经过去接近2年了，之前对这个芯片一直不是很了解，只是会用用别人封装好的API，对其怎么实现的、涉及到的寄存器、初始化流程都完全不了解，这就导致如果在使用中出现了问题，我是没办法分析问题来源的。因此，如果想要自如地使用某芯片，一定要学会看芯片的使用手册、寄存器手册等等

1.一些常见问题：

0.MPU6050测的是3轴加速度和角速度，不是角加速度。。。

1.MPU6050的SDK由2部分组成，自底向上是inv_mpu6050、dmp_motion_driver，前者偏向于底层，主要是器件本身的初始化；后者是应用层，主要是对传感器的姿态解算的相关设置和计算

2.DMP库里面FIFO的长度只设置了32，而官方文档该寄存器(不该称为寄存器啊)容量是1024字节的，是不是它FIFO的容量没有用满呢？

nonono，这个FIFO不是你设置它的容量的，他只是一次从里面读32个数据，我看错了。而且那个读写寄存器的意思是从FIFO的尾部读取一个字节的数据，而不是这个寄存器就是FIFO。相当于对FIFO进行一次.pop()拿出尾部的数据，并且FIFO的长度以及里面每一个字节的数据是什么都由代码决定。

3.MPU6050的DMP也有很多寄存器，但是地址没有直接写在6050的寄存器手册上。但是SDK中有使用到了的DMP的寄存器的地址。

4.虽然6050寄存器手册上写了FIFO中可以存放任何传感器的数据比如：温度、角加速度等等。但是如果用了DMP的话，FIFO中可以存放的数据一般只有四元数、角(加)速度，具体FIFO存什么是由dmp_enable_feature()这个函数决定的，DMP会根据这个函数的参数来决定把什么数据输入FIFO。也就是说FIFO中的数据不是6050本身采集到的数据然寄存器序号直接丢进去，而是由DMP来控制了。

1
2
3
4
5
6
7
8
9
dmp.packet_length = 0;
if (mask & DMP_FEATURE_SEND_RAW_ACCEL)
    dmp.packet_length += 6;
if (mask & DMP_FEATURE_SEND_ANY_GYRO)
    dmp.packet_length += 6;
if (mask & (DMP_FEATURE_LP_QUAT | DMP_FEATURE_6X_LP_QUAT))
    dmp.packet_length += 16;
if (mask & (DMP_FEATURE_TAP | DMP_FEATURE_ANDROID_ORIENT))
    dmp.packet_length += 4;

最后每次从FIFO中读取dmp.packet_length长度的数据

2.如何自己选择要从DMP中获取什么数据？

先看看DMP的SDK中定义的一些结构体

1
2
3
4
5
6
7
8
9
struct dmp_s
{
    void (*tap_cb)(unsigned char count, unsigned char direction);
    void (*android_orient_cb)(unsigned char orientation);
    unsigned short orient;
    unsigned short feature_mask;
    unsigned short fifo_rate;
    unsigned char packet_length;
};

这个结构体里的feature_mask变量是告诉DMP获取什么数据 的关键。在6050的初始化中，会由这个函数

1
2
3
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);

来设置dmp的feature_mask和packet_length这2个参数。后者就是调用一次dmp_read_fifo()从FIFO中拿出的字节长度。

进行设置完后，每次只要用dmp_read_fifo()这个API就可以得到4元数、acc、gyro等数据(如果你设置了相应的feature_mask的话)，根据4元数我们可以转换成对应的rpy角

3.MPU6050的中断系统

DMP库中开启了这个寄存器的最低位，也就是DATA_Ready_EN，每当所有传感器数据写入FIFO时，就会触发中断。为了避免FIFO溢出，最好的方法就是在这个中断里面把数据从FIFO读出来，如果是在定时器中断里面以一定的周期读数据，那么总是会发生FIFO溢出的。除非读取频率要高于采样率。

4.数据的读取

从MPU6050读取数据有2种方式：

  • 直接从数据寄存器里读

  • 从FIFO里面读

如果不打算用DMP库的话，可以直接从数据寄存器里面读数据

4.1原始数据的处理

从6050读出acc和gyro都是内部16bit的传感器采集的模拟量经过ADC输出的值。所以不能直接用，需要转换一下。设读出的原始数据为ADCx

法1：实际值 = ADCx*量程/32768

32768是2的15次方，因为内部ADC是16位且有符号，所以除以这个。

ADCx的单位实际上是LSB，因此也可以这样计算实际值

法2：实际值 = ADCx / 灵敏度，比如量程是16g，那么灵敏度就是2048，量程越小越精确

DMP库的量程设置在int mpu_init(void)这个函数中，它的设置：

1
2
3
4
*  Gyro FSR: +/- 2000DPS\n
*  Accel FSR +/- 2G\n
*  DLPF: 42Hz\n
*  FIFO rate: 50Hz\n

经单位换算后，可以发现MPU6050的角速度、加速度是比较准的，不向4元数那么飘

4.2读取数据的时间

刚才也提到了，可以利用数据就绪中断或者定时器的方式来读数据，经测试数据就绪中断的频率和采样率不同，会低于采样率，且其频率一直在变，因此用定时器中断读取数据更好

5.姿态解算

姿态解算的实际上就是根据旋转变换矩阵R，求出对应的R、P、Y角。由于解这几个角涉及到了很多三角运算，为了简化运算，采用了四元数表示。

求出旋转变换矩阵R是姿态解算的关键，R可以看成是关于运行时间t的一个微分方程，它和陀螺仪w有关。一般求R是通过积分的方法求得。
这里Q就是旋转矩阵R的四元数形式。

姿态解算中，通过迭代的方式求解Q

这就是根据时间更新R的公式，q一般取初值(1,0,0,0)

由上式可以看出，姿态解算准确的关键就是陀螺仪的数据w准确

6.滤波