b站铁头山羊的STM32学习笔记7-MPU6050(姿态解算)

表示物体当前运动姿态的东西：欧拉角

欧拉角概念

与传统的直角坐标系不同，这个坐标记录的是绕着Z轴，X轴，Y轴的逆时针旋转角度，三个角度确定了，当前运动姿态也就确定了，

MPU6050对欧拉角的测量方法

加速度计

可以测量当前物体沿着x,y,z轴的加速度，飞机重力(感觉说合力也可以)向下，正对着飞机做一个受力分析，也就是主视图：重力向z轴和y轴分别投影az，ay，粉色线与z轴夹角就是绕着x轴逆时针旋转的角度(翻滚角roll)，可以通过arctan(ax/az)得到，同理在右视图中arctanx(ax/ax)也可以得到粉色线与绿色线夹角，但他是顺时针的角度，所以是俯仰角(绕着y轴逆时针旋转pitch)的相反数，也就是说

roll(翻滚角)=arctan(ax/az)

pitch(俯仰角)=-arctanx(ax/az)

加速度计无法测量偏航角(绕z轴旋转的角度)

缺点：空气中有不断变化的各种各样的阻力(外部震动)，会导致加速度抖动，在图形上就是有毛刺，并且没有办法测量偏航角

陀螺仪

陀螺仪可以测量各个轴上旋转的角速度

将一个轴的旋转角度随着时间变化记录下来，就是一个函数

新时间角度=旧时间角度+角度速*Δt

应该就是角速度对t积分可以得到旋转角度关于t的函数

缺点：新时间角度=旧时间角度+角度速*Δt得到的新角度与真实新时间角度有差异，虽然很小，但这个算出来的新时间角度会被用来计算下一个新新时间的角度，差异会累积下去并且不断累积新的差异，导致误差越来越大，称为"漂移"

传感器融合

加速度计和陀螺仪有各自的优缺点

加速度计测量得到的是每个时刻具体的加速度，不存在误差累计的情况，但会受到外力震动的影响导致加速度快速变化，用这个计算得到的角度也有毛刺，且无法测量偏航角

陀螺仪不会受到外力震动的影响，但存在误差累积的情况，也就是会"漂移"

将这两个数据融合一下可以得到较为真实的角度

融合后结果d=α*陀螺仪测量结果+(1-α)*加速度计测量结果，α=τ/(τ+Δt)，Δt=传感器采样的时间间隔，例如MPU6050采样频率为200HZ，则Δt=5ms，τ铁头山羊说一般取0.1，原因不知道，弹幕有人提到这叫"一阶低通滤波"，还有人说协方差(卡尔曼滤波)更好，暂时看不懂，怀疑是通信原理专业的课程，没学过

项目开始

搭建电路

MPU6050测量的是模拟量，内置ADC转换成数字量，然后用I2C传给单片机

VCC接3.3V，GND接地，SCL和SDA是开漏输出接连I2C总线，2C要接上拉电阻，XDA和XCL图里有介绍，是用来拓展的，可以将6轴拓展为9轴，接磁力计，也就是磁力计获取的数据用XDA和XCL传输，这里没用到，悬空即可，AD0可以用于地址选择，MPU6050的前6位地址是固定的b1101000,最后一位地址与d0相同，I2C从机地址共7位，INT是中断，每收集到一次数据就通过INT引脚发送一个脉冲，这里没用到

这里的400k是使用I2C通信的波特率(码元传输速度，也就是通信的快慢)我一开始和那个MPU6050的采样频率搞混了，MPU6050采样频率是200HZ，也就是5ms采样一次，400k是信息传递速度，差了2000倍呢，DutyCycle指的是占空比，可以是2:1或者16:9，选哪一个要看芯片的数据手册(9.2节是I2C接口)，没写就用默认的2:1

整体PAL库代码编写

编程思路与代码框架

裸机多任务模型分为初始化，进程函数以及中断服务函数，这里没用到中断，主要是初始化以及进程函数，初始化函数用来对MPU6050初始化，设置参数，进程函数用来获取传感器数据并更新当前的欧拉角，采集到的传感器数据包括三个加速度，三个角速度，一个温度，这些放在静态变量里，还有欧拉角也放在静态变量里，在进程函数中，采集到的七个数据放入静态变量，然后这三个采集到的加速度算出加速计的两个欧拉角放入欧拉角_a中，三个采集到的角速度和原来的欧拉角算出欧拉角_g，(两个欧拉角  _a和_g都是临时变量)，最后将这两个欧拉角按照α那个公式融合得到新的欧拉角

也就是说，初始化函数用于初始化，在main里进行一次，进程函数用于读取传感器数据并算出新的欧拉角，最后的那个获取结果好像只是把得到的传感器原始数据和最新的欧拉角以及温度放入了四个数组，看代码应该是用来显示的，但铁头山羊给的代码只有pal的没有真写出来。

那个PAL开头的意思是每5ms执行一次这个函数，因为MPU6050的采样率设置为了200HZ，也就是每5ms传来一次数据，所以进程函数也需要每5ms处理并更新一次数据

初始化代码

使用I2C1模块，波特率为400KHZ，占空比为默认的2:1，采样率为200HZ也就是5ms采集一次数据，与进程函数每5ms读取并处理更新一次欧拉角对应，量程则是处理函数中/16384有关

进程函数代码

获取结果代码

MPU6050写数据和读数据代码

上面代码是铁头山羊用PAL库写的，MPU6050初始化和从MPU6050读取数据都是已经封装好的了，以下是一些零碎代码，用来实现MPU6050写数据和读数据，它们可以用来对MPU6050进行初始化(写数据)和读取采集到的七个数据(读数据)

寄存器介绍

寄存器整体表

地址为0x6b的寄存器可以进行复位和唤醒，位7写入1则复位，复位后清0，位6写1后进入休眠状态，写0进入唤醒状态，复位后默认为休眠状态

地址为0x19的寄存器可以设置采样率，位0~位7是一个整体(SMPLRT)

采样率=陀螺仪输出频率/(SMPLRT_DIV+1)

地址为0x1c的寄存器的位3和位4是AFS_SEL，有四个状态，分别对应不同的量程，正负2g指的是量程为正负两倍的重力加速度，特别注意这个是带符号的，所以采集到的十六位数据是带符号整型数据，范围在-32768~+32767，+2g对应的是+32767

地址为0x1b的寄存器的位3和位4是FS_SEL，有四个状态，分别对应不同的量程，正负2000度指的是旋转角度量程为正负2000度，特别注意这个是带符号的，所以采集到的十六位数据是带符号整型数据，范围在-32768~+32767，+2000度对应的是+32767

寄存器操作接口

写寄存器

向从机地址为0xd0的地址发送数据，发两个，合起来是三个数据：发送从机地址0xd0，发送寄存器地址0x19，寄存器要写的值(要写的数据放入数组中，然后发送长度为2，一个是0x19，一个是值)

批量写入

向从机地址为0xd0的地址发送数据，发n个，合起来是n+1个数据：发送从机地址0xd0，发送寄存器首地址0x19，寄存器要依此写的值(要写的数据放入数组中，然后发送长度为n，一个地址，n-1个值，之前还有一个从机地址共n+1个)

读寄存器

从从机地址为0xd0的地址读取数据，读2个：先发送两个数据：发送从机地址0x19，发送寄存器首地址0x1a，合起来发送两个数据，然后读取1个数据，读取后放入data

批量读入

从从机地址为0xd0的地址读取数据，读2个：先发送两个数据：发送从机地址0x19，发送寄存器首地址0x3b，合起来发送两个数据，然后读取2个数据，读取后首地址为data_recv指向的内存空间，长度为2，依此放入(就是把读取到的数据放入了data_recv)，先收到的是0x3b寄存器的值，ax高字节，第二个是0x3c寄存器的值，ax低字节；高字节data_recv[0]左移八位在和data_recv[1]低字节或得到完整的16位带符号整形数据，范围在-32768~+32767之间，需要/16384来转化成对应的加速度

初始化

流程图

代码

读取数据的换算方法数

例如ax：先收到的是0x3b寄存器的值，ax高字节，第二个是0x3c寄存器的值，ax低字节；buffer[0]左移八位在和buffer[1]或得到完整的16位带符号整形数据，范围在-32768~+32767之间，需要/16384来转化成对应的加速度，剩余同理，共七个数据，每个数据16位占14个寄存器，加速度和陀螺仪计算方法类似，温度看不懂