MPU6050陀螺仪与Processing和上位机飞控联动实录

简而言之，MPU6050 = 三轴MEMS陀螺仪 + 三轴MEMS加速度计 + 可扩展数字运动处理器DMP ，它可进行姿态解算（Pitch、Yaw、Roll角），我们还可以外接Processing IDE，或外接匿名上位机(V7)，实时绘制系统的飞行姿态，下面讲一下整个联调过程以及遇到的坑。

图0 单片机与上位机(V7)飞行姿态联动

一，MPU6050简介

MPU6050是InvenSense推出的集成6轴运动处理组件，即三轴MEMS（注1）陀螺仪传感器和三轴MEMS加速度传感器，相较于多组件方案，集成模块可以免除各个组件时间轴之差的问题，还能大大减小封装的空间。它含有一个副IIC接口，可用于连接外部磁力传感器，利用自带数字运动处理器（DMP，Digital Motion Processor的缩写）硬件加速引擎，通过主IIC接口，可以向应用端输出完整的9轴姿态融合演算数据。

注1：MEMS是微机电系统（Micro Electro Mechanical System）的英文缩写。它是指可批量制作的，集微型机构、微型传感器、微型执行器以及信号处理和控制电路、通信和电源于一体的系统。比较成熟的MEMS传感器有三种：加速度计、压力传感器和陀螺仪。

有了DMP，我们可以使用InvenSense提供的运动处理资料库，非常方便地实现姿态解算，降低了运动处理运算对操作系统的负荷，同时大大降低了开发难度。

二，MUP6050特点

MPU6050 的特点有：

① 以数字形式输出 6 轴或 9 轴（需外接磁传感器）（注2）的旋转矩阵、四元数(quaternion)、欧拉角格式(Euler Angle forma)的融合演算数据（需 DMP 支持）。

② 具有 131 LSBs/°/sec 敏感度与全格感测范围为±250、±500、±1000 与±2000°/sec 的 3 轴角速度感测器(陀螺仪)。

③ 集成可程序控制，范围为±2g、±4g、±8g 和±16g 的 3 轴加速度传感器。

④ 移除加速器与陀螺仪轴间敏感度，降低设定给予的影响与感测器的飘移。

⑤ 自带数字运动处理引擎可减少MCU复杂的融合演算数据、感测器同步化、姿势感应等的负荷。

⑥ 内建运作时间偏差与磁力感测器校正演算技术，免除了客户须另外进行校正的需求。

⑦ 自带一个数字温度传感器。

⑧ 带数字输入同步引脚(Sync pin)支持视频电子影相稳定技术与 GPS。

⑨ 可程序控制的中断(interrupt)，支持姿势识别、摇摄、画面放大缩小、滚动、快速下降中断、high-G 中断、零动作感应、触击感应、摇动感应功能。

⑩ VDD 供电电压为 2.5V±5%、3.0V±5%、3.3V±5%；VLOGIC 可低至 1.8V± 5%。

⑪ 陀螺仪工作电流：5mA。

⑫ 自带 1024 字节 FIFO，有助于降低系统功率。

⑬ 400Khz 的 IIC 通信接口。

注2：三轴 = 3轴陀螺仪

六轴 = 3轴加速度计 + 3轴陀螺仪

九轴 = 3轴加速度计 + 3轴陀螺仪 + 3轴磁力计

三，原理说明

3.1.MEMS陀螺仪

MEMS陀螺仪与传统的陀螺仪原理不同。传统陀螺仪是一个不停转动的物体即转子，由于惯性其旋转轴的指向不会随着承载它的支架旋转而改变方向，如下图所示，三轴即横滚轴、俯仰轴和航向轴，通俗点说也就是X Y Z三轴，当三轴发生旋转时，旋转轴是不会随之变化的。显然，将这样一个不停旋转的装置用微机技术在硅片衬底上加工是非常难的事情。

图1 传统陀螺仪示意

为此，MEMS陀螺仪基于陀螺仪的特性利用科里奥利力来实现。科里奥利力即科氏力，它是对旋转体系中进行直线运动的质点由于惯性相对于旋转体系产生的直线运动的偏移的一种描述。

以MEMS陀螺仪常见的微机械音叉式陀螺结构为例，陀螺仪中间有一个悬在空中的质量块，它可以在两个互相垂直的平面内振动。在一般状态下，陀螺仪通过加震荡电压迫使质量块在其中一个平面做振动，如下图蓝色箭头所示，也就是提供了科里奥利力中的径向运动。在陀螺仪发生旋转时，质量块受到与它运动方向垂直的科里奥利力，产生了另一个平面的振动，如下图黄色箭头所示，根据振动产生的电容变化便可以测量科里奥利力的大小，又因为科里奥利力正比于角速度，所以我们可以通过电容变化计算得到角速度。

图2 科里奥利力示意

3.2.MEMS加速度计

技术成熟的MEMS加速度计分为三种：压电式、容感式和热感式。

压电式：在其内部有一个刚体支撑的质量块，在运动的情况下质量块会产生压力，刚体产生应变，从而转换为电信号输出。

容感式：它是标准的平行板电容器，加速度的变化带来质量块的移动从而改变电容两级的间距以及面积，通过计算即可得到相应的加速度。

热感式：內部无任何质量块，它的中央有加热体，周边是温度传感器，里面是密闭的气腔。工作时在加热体的作用下，气体在内部形成一个热气团，热气团的比重和周围的冷气是有差异的，通过惯性热气团的移动形成的质量块，热场变化让感应器感应到加速度值。

附录A：

陀螺仪历史发展和原因参考资源：https://zhuanlan.zhihu.com/p/610287151

四，模块初始化工作

4.1.初始化 IIC 接口

MPU6050 采用 IIC 与开发板通信，所以我们需要先初始化与 MPU6050 连接的 SDA 和 SCL 数据线。

4.2.复位 MPU6050

这一步让 MPU6050 内部所有寄存器恢复默认值，通过对电源管理寄存器 1（0x6B）的bit7 写 1 实现。复位后，电源管理寄存器 1 恢复默认值(0x40)，然后必须设置该寄存器为0x00，以唤醒 MPU6050，进入正常工作状态。

const uint8_t MPU6050_REGISTER_PWR_MGMT_1 = 0x6B;
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_1, 0x01);

4.3.设置角速度传感器（陀螺仪）和加速度传感器的满量程范围

这一步，我们设置两个传感器的满量程范围（FSR）（注3），分别通过陀螺仪配置寄存器（0X1B）和加速度传感器配置寄存器（0X1C）设置。我们一般设置陀螺仪的满量程范围为 ±2000dps（注4），加速度传感器的满量程范围为 ±2g。

注3：量程是度量工具的测量范围，也就是仪器设备所能测量的物理量的最大值。

注4：dps是角速度单位，即degree per second的缩写。

（1）陀螺仪配置

寄存器地址：0X1B（英文对应于Gyroscope Configuration）

写入数据：0X00，选择量程为：±250dps，视情况而定，可以为±250、±500、±1000、±2000（°/sec）

mpu.setGyroRange(MPU6050_RANGE_250_DEG);//Adafruit_MPU6050库的写法

（2）加速度计配置

寄存器地址：0X1C（英文对应于Accelerometer Configuration）

写入数据：0X00，选择量程为：±2g（视情况而定）

mpu.setAccelerometerRange(MPU6050_RANGE_2_G);//Adafruit_MPU6050库的写法

4.4.设置其他参数

我们还需要配置的参数有：

关闭中断
关闭 AUX IIC 接口
禁止 FIFO
设置陀螺仪采样率
设置数字低通滤波器（DLPF）等。

我们不用中断方式读取数据，所以可以关闭中断。也没用到 AUX IIC 接口外接其他传感器，所以也关闭这个接口。如下所示，分别通过中断使能寄存器（0x38）和用户控制寄存器（0x6A）控制。

const uint8_t MPU6050_REGISTER_INT_ENABLE = 0x38; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_INT_ENABLE, 0x01); 
const uint8_t MPU6050_REGISTER_USER_CTRL = 0x6A; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_USER_CTRL, 0x00);

MPU6050 可以使用 FIFO 存储传感器数据，不过我们也可以不用，所以关闭所有 FIFO 通道。如下所示，通过 FIFO 使能寄存器（0x23）控制，默认都是 0（即禁止 FIFO），所以用默认值就可以了。

const uint8_t MPU6050_REGISTER_FIFO_EN = 0x23; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_FIFO_EN, 0x00);

阅读寄存器说明手册可知，MPU6050的陀螺输出频率可达8kHz，加速度计为1kHz，而且可以通过分频来降低频率。采样频率就是通过陀螺仪输出频率分频得到的。陀螺仪采样率通过采样率分频寄存器（0x19）控制。如果要得到200Hz的采样率，那么分频值就是39。只要设置寄存器25的值为39，就可以使得DMP以200Hz来更新寄存器中的数据，只要按时读取寄存器就可以了（注5）。

const uint8_t MPU6050_REGISTER_SMPLRT_DIV = 0x19; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SMPLRT_DIV, 0x07);

注5：采样频率分频解释参考 https://zhuanlan.zhihu.com/p/21670600

数字低通滤波器(DLPF)则通过配置寄存器（0x1A）设置，一般设置 DLPF 为带宽的 1/2 即可。

const uint8_t MPU6050_REGISTER_CONFIG = 0x1A; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_CONFIG, 0x00);

4.5.配置系统时钟源并使能角速度传感器和加速度传感器

可选的时钟源有三种：内部8MHz晶振、基于陀螺仪的时钟和外部时钟源。默认为内部晶振，强烈建议选择其中一个陀螺仪（或者外部时钟源）作为时钟源，以提高稳定性。

系统时钟源同样是通过电源管理寄存器 1（0X6B）来设置，该寄存器的最低三位用于设置系统时钟源选择，默认值是 0（内部晶振），不过我们一般设置为 1，即选择 X 轴陀螺 PLL 作为时钟源，以获得更高精度的时钟。

寄存器地址：0x6B（英语对应于Power Management 1）

写入数据：0x01，选择陀螺仪的 X 轴作为时钟源

同时，使能角速度传感器和加速度传感器，这两个操作通过电源管理寄存器 2（0x6C）来设置，设置对应位为 0 即可开启，如下所示。

const uint8_t MPU6050_REGISTER_PWR_MGMT_2 = 0x6C; 
I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_2, 0x00);

至此，MPU6050 的初始化就完成了，可以正常工作了（其他未设置的寄存器全部采用默认值即可）。那么当代码烧录后，MPU的参考点是什么呢？参考点其实就是MPU6050初始化之后一开始的位置，没有规定说哪一个方向就是基准点，所以初始化之后的初始位置就是(0,0,0)点。

接下来，我们就可以读取相关寄存器，来得到加速度传感器、角速度传感器和温度传感器的数据了。

五，数据处理

5.1.原始数据格式

我们感兴趣的数据位于 0X3B 到 0X48 这14个字节的寄存器当中。这些数据会被动态更新，下面是相关的寄存器地址与数据名称。注意每个数据都是2个字节，高位在前低位在后。

0x3B，加速度计的X轴分量ACC_X

0x3D，加速度计的Y轴分量ACC_Y

0x3F，加速度计的Z轴分量ACC_Z

0x41，当前温度TEMP

0x43，绕X轴旋转的角速度GYR_X

0x45，绕Y轴旋转的角速度GYR_Y

0x47，绕Z轴旋转的角速度GYR_Z

关于模块的坐标系定义如下图所示，将模块面朝自己，此时水平方向即为X轴，竖直方向为Y轴，指向自己的方向为Z轴。

图3 可根据右手螺旋定则确定方向

5.2.DMP处理成欧拉角

MPU6050 自带了数字运动处理器，即 DMP。而且 InvenSense 提供了一个运动驱动库，结合 DMP，可以将我们的加速度传感器和角速度传感器的原始数据，直接转换成四元数输出，而得到四元数之后，就可以很方便地计算出欧拉角：航向角（yaw，也叫偏航角）、横滚角（roll）和俯仰角（pitch）（注6）。

注6：欧拉角就是物体绕坐标系三个坐标轴(x,y,z轴）的旋转角度。这三个角的动画解释参考：https://zhuanlan.zhihu.com/p/228805569

DMP 输出的四元数是 q30 格式的，也就是浮点数放大了 2 的 30 次方倍。在换算成欧拉角之前，必须先将其转换为浮点数，也就是除以 2 的 30 次方，然后再进行计算，计算公式为：

q0=quat[0] / q30;//q30格式转换为浮点数 
q1=quat[1] / q30; 
q2=quat[2] / q30; 
q3=quat[3] / q30; //计算得到ypr：pitch俯仰角/roll横滚角/yaw航向角 
pitch=asin(-2 * q1 * q3 + 2 * q0* q2)* 57.3;//俯仰角 
roll=atan2(2 * q2 * q3 + 2 * q0 * q1, -2 * q1 * q1 - 2 * q2* q2 + 1)* 57.3;//横滚角 
yaw=atan2(2*(q1q2 + q0q3),q0q0+q1q1-q2q2-q3q3) * 57.3;//航向角

上面代码中，quat[0]～quat[3]是 MPU6050 的 DMP 解算后的四元数，q30 格式，所以要除以一个2 的 30 次方。其中 q30 是一个常量：1073741824，即 2 的 30 次方，然后带入公式，计算出欧拉角。上述计算公式的 57.3 是弧度转换为角度，即 180/π，这样得到的结果就是以度（°）为单位的。关于四元数与欧拉角的公式推导，此处不再赘述。

需要注意的是，单靠 MPU6050 无法准确得到 yaw 角，需要和地磁传感器结合使用，具体原因请看下一节。

5.3.Yaw角的问题

因为没有参考量，所以无法求出当前的Yaw角的绝对角度，只能得到Yaw的变化量，也就是角速度GYR_Z。当然，我们可以通过对GYR_Z积分的方法来推算当前Yaw角（以初始值为准），但由于测量精度的问题，推算值会发生漂移，一段时间后就完全失去意义了。如果必须要获得绝对的Yaw角，那么应当选用MPU9250这款九轴运动跟踪芯片，它可以提供额外的三轴罗盘数据，这样我们就可以根据地球磁场方向来计算Yaw角了。

5.4.数据校准和滤波

MPU6050提供的数据夹杂着较严重的噪音，在芯片处理静止状态时数据摆动都可能超过2%。除了噪音，各项数据还会有偏移的现象，也就是说数据并不是围绕静止工作点摆动，因此要先对数据偏移进行校准，再通过滤波算法消除噪音。

（一）校准

校准是比较简单的工作，我们只需要找出摆动的数据围绕的中心点即可。以GRY_X为例，在芯片处理静止状态时，这个读数理论上讲应当为0，但它往往会存在偏移量，比如我们以10ms的间隔读取了10个值如下：

-158.4, -172.9, -134.2, -155.1, -131.2, -146.8, -173.1, -188.6, -142.7, -179.5

这10个值的均值，也就是读数的偏移量为-158.25。在获取偏移量后，每次的读数都减去偏移量就可以得到校准后的读数了。当然这个偏移量只是估计值，比较准确的偏移量要对大量的数据进行统计才能获知，数据量越大越准，但统计的时间也就越慢。一般校准可以在每次启动系统时进行，我们应当在准确度和启动时间之间做一个权衡。

三个角速度读数GYR_X、GYR_Y和GYR_Z均可通过统计求平均的方法来获得，但三个加速度分量就不能这样简单的完成了，因为芯片静止时的加速度并不为0。

加速度值的偏移来自两个方面，一是由于芯片的测量精度，导至它测得的加速度向量并不垂直于大地；二是芯片在整个系统（如无人机）上安装的精度是有限的，系统与芯片的座标系很难达到完美重合。前者我们称为读数偏移，后者我们称为角度偏移。

由于校准角度偏移需要专业设备，且对于一般应用来说，两步校准带来的精度提升并不大，因此通常只进行读数校准即可。读数校准办法如下所示。

首先将MPU6050牢牢地固定在系统底座上，并使二者座标系尽可能地重合。其次将系统置于水平、坚固的平面上，并充分预热。此时，我们认为芯片的加速度方向应当与Z轴负方向重合，且加速度向量的模长为g，因此ACC_X和ACC_Y的理论值应为0，ACC_Z的理论值应为-16384（假设我们设定2g的倍率，1g的加速度的读数应为最大值-32768的一半），即在统计偏移量的过程中，ACC_Z每次读数都要加上16384，再进行统计均值校准，如下代码所示。

  float valSums[7] = {0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0f, 0.0};
  //先求和
  for (int i = 0; i < nCalibTimes; ++i) {
    int mpuVals[nValCnt];
    ReadAccGyr(mpuVals);
    for (int j = 0; j < nValCnt; ++j) {
      valSums[j] += mpuVals[j];
    }
  }
  //再求平均
  for (int i = 0; i < nValCnt; ++i) {
    calibData[i] = int(valSums[i] / nCalibTimes);
  }
  calibData[2] += 16384;

（二）卡尔曼滤波

对于夹杂了大量噪音的数据，卡尔曼滤波器的效果无疑是最好的。如果不想考虑算法细节，可以直接使用Arduino的Klaman Filter库完成（注7）。在我们的模型中，一个卡尔曼滤波器接受一个轴上的角度值、角速度值以及时间增量，估计出一个消除噪音的角度值。跟据当前的角度值和上一轮估计的角度值，以及这两轮估计的间隔时间，我们还可以反推出消除噪音的角速度。

注7：Klaman源代码中文注释参见：https://blog.csdn.net/acktomas/article/details/89087174

六、接线方式

不需要都接。一般只需要这几个：

模块 : 开发板

VCC : 5V电源

GND : GND

SCL : 主IIC时钟线(如下图所示，一般接NodeMCU的D6)——C代表Clock，控制数据发送的时序。

SDA : 主IIC数据线(如下图所示，一般接NodeMCU的D7)——D代表Data，用来传输数据的。

AD0 : 从机地址最低位。模块电路中将AD0下拉接地，所以不接其他外部信号。I2C从机地址（手册可以查看到），对于MPU6050来说固定为0x68（某些批次可能为0x98），所以可以用于验证I2C读出协议是否正常。也可以调整AD0引脚电压就可以改变其从机地址：AD0接3V的时候（AD0=1)，从机地址为0x69。

图4 一种接线方式

上述接线方式能用下述简单实验代码烧录成功和跑通：

#include <Wire.h>

// MPU6050 Slave Device Address
const uint8_t MPU6050SlaveAddress = 0x68;//MPU6050的I2C地址

// Select SDA and SCL pins for I2C communication 
const uint8_t scl = D6;
const uint8_t sda = D7;

// sensitivity scale factor respective to full scale setting provided in datasheet 
const uint16_t AccelScaleFactor = 16384;
const uint16_t GyroScaleFactor = 131;

// MPU6050 few configuration register addresses
const uint8_t MPU6050_REGISTER_SMPLRT_DIV   =  0x19;
const uint8_t MPU6050_REGISTER_USER_CTRL    =  0x6A;
const uint8_t MPU6050_REGISTER_PWR_MGMT_1   =  0x6B;
const uint8_t MPU6050_REGISTER_PWR_MGMT_2   =  0x6C;
const uint8_t MPU6050_REGISTER_CONFIG       =  0x1A;
const uint8_t MPU6050_REGISTER_GYRO_CONFIG  =  0x1B;
const uint8_t MPU6050_REGISTER_ACCEL_CONFIG =  0x1C;
const uint8_t MPU6050_REGISTER_FIFO_EN      =  0x23;
const uint8_t MPU6050_REGISTER_INT_ENABLE   =  0x38;
const uint8_t MPU6050_REGISTER_ACCEL_XOUT_H =  0x3B;//我们感兴趣的数据位于 0X3B 到 0X48 这14个字节的寄存器当中
const uint8_t MPU6050_REGISTER_SIGNAL_PATH_RESET  = 0x68;

int16_t AccelX, AccelY, AccelZ, Temperature, GyroX, GyroY, GyroZ;

void setup() {
  Serial.begin(115200);
  Wire.begin(sda, scl);
  MPU6050_Init();
}

void loop() {
  double Ax, Ay, Az, T, Gx, Gy, Gz;
  
  Read_RawValue(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_XOUT_H);
  
  //divide each with their sensitivity scale factor
  Ax = (double)AccelX/AccelScaleFactor;
  Ay = (double)AccelY/AccelScaleFactor;
  Az = (double)AccelZ/AccelScaleFactor;
  T = (double)Temperature/340+36.53; //temperature formula
  Gx = (double)GyroX/GyroScaleFactor;
  Gy = (double)GyroY/GyroScaleFactor;
  Gz = (double)GyroZ/GyroScaleFactor;

  Serial.print("Ax: "); Serial.print(Ax);
  Serial.print(";Ay: "); Serial.print(Ay);
  Serial.print(";Az: "); Serial.print(Az);
  Serial.print(";T: "); Serial.print(T);
  Serial.print(";Gx: "); Serial.print(Gx);
  Serial.print(";Gy: "); Serial.print(Gy);
  Serial.print(";Gz: "); Serial.println(Gz);

  delay(100);
}

void I2C_Write(uint8_t deviceAddress, uint8_t regAddress, uint8_t data){
  Wire.beginTransmission(deviceAddress);
  Wire.write(regAddress);
  Wire.write(data);
  Wire.endTransmission();
}

// read all 14 register
void Read_RawValue(uint8_t deviceAddress, uint8_t regAddress){
  Wire.beginTransmission(deviceAddress);
  Wire.write(regAddress);
  Wire.endTransmission();
  Wire.requestFrom(deviceAddress, (uint8_t)14);
  AccelX = (((int16_t)Wire.read()<<8) | Wire.read());
  AccelY = (((int16_t)Wire.read()<<8) | Wire.read());
  AccelZ = (((int16_t)Wire.read()<<8) | Wire.read());
  Temperature = (((int16_t)Wire.read()<<8) | Wire.read());
  GyroX = (((int16_t)Wire.read()<<8) | Wire.read());
  GyroY = (((int16_t)Wire.read()<<8) | Wire.read());
  GyroZ = (((int16_t)Wire.read()<<8) | Wire.read());
}

//configure MPU6050
void MPU6050_Init(){
  delay(150);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SMPLRT_DIV, 0x07);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_1, 0x01);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_PWR_MGMT_2, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_CONFIG, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_GYRO_CONFIG, 0x00);//set +/-250 degree/second full scale
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_ACCEL_CONFIG, 0x00);// set +/- 2g full scale
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_FIFO_EN, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_INT_ENABLE, 0x01);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_SIGNAL_PATH_RESET, 0x00);
  I2C_Write(MPU6050SlaveAddress, MPU6050_REGISTER_USER_CTRL, 0x00);
}

但是需要注意，这时候我们常用来遍历打印I2C设备地址的Scanning-I2C-Device程序就报告找不到I2C设备。

所以为了找到I2C设备，为了能使用Adafruit_MPU6050开源库，我们采用了下面这种NodeMCU+MPU6050的接线方式：

VCC VU (5V USB) Not available on all boards so use 3.3V if needed.

GND G Ground

SCL D1 (GPIO05) I2C clock

SDA D2 (GPIO04) I2C data

XDA not connected

XCL not connected

AD0 not connected

INT D8 (GPIO15) Interrupt pin

只有按上述方式接线，Scanning-I2C-Device程序才会打印说找到了I2C设备：

Scanning...

I2C device found at address 0x68 !

Adafruit_MPU6050库的样例代码也才能找到芯片。

附录B：

引脚讲解和简单示例代码可参考：https://docs.wokwi.com/parts/wokwi-mpu6050

通常我们为了方便把IIC设备分为主设备和从设备，基本上谁控制时钟线（即控制SCL的电平高低变换）谁就是主设备。

七，外接Processing实验

Processing IDE是一款强大的，能与其他软件尤其是硬件如Arduino、STM32和ESP32直接交互的编程语言和开发环境。

为了能让MPU6050+NodeMCU的串口输出能够控制Processing软件里的小飞机模型，我们需要准备好以下库：

1）Arduino IDE需要安装如下库或者直接把库文件复制到你的文件夹下直接引用：

https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU6050

2）Processing IDE需要安装如下库：

https://github.com/postspectacular/toxiclibs

接下来开始运行。

在Arduino IDE中打开 MPU6050_DMP6 的示例，注释掉其他选项，只保留下面这一个选项：

#define OUTPUT_READABLE_YAWPITCHROLL

烧录之后，你会看到串口打印了yaw、pitch、roll三个角的实时变化：

MPU6050 connection successful

Send any character to begin DMP programming and demo:

Initializing DMP...

.****************.***....>......-972.00000, -429.00000, 4374.00000, 96.00000, 16.00000, -34.00000

Enabling DMP...

DMP ready! Waiting for first interrupt...

ypr 24.96 -0.83 -11.36

ypr 23.79 -0.54 -9.71

ypr 23.21 -0.22 -8.21

ypr 22.98 -0.17 -6.86

ypr 22.87 -0.41 -5.73

ypr 22.63 -0.72 -4.76

ypr 21.65 -1.02 -3.73

ypr 20.10 -1.19 -2.59

ypr 18.38 -1.10 -1.33

接下来我们要与Processing IDE配合了。

在 MPU6050_DMP6 中注释掉其他选项，只保留下面这一个选项：

#define OUTPUT_TEAPOT

烧录上传。同时用Processing IDE打开 MPUTeapot.pde，运行起来，Teapot程序会自动给串口发送一个字符让MPU6050跑起来。此时，正值MPU初始化阶段，你的MPU模块一定要水平地平放在稳固的平面上，如下图所示，MPU平放在你面前，亮灯的地方就是机尾，机头方向就是X轴，垂直于模块向上就是Z轴。放得不平，或者方向不对，小飞机就会飞得奇奇怪怪。

图5 模块的机头机尾方向

初始化成功后，模块的机头向上，Processing IDE里的小飞机也机头向上了，如下图所示。

图6 MPU+Processing第一次联动

如果用 MPUplane.pde 配合，还可以绘制出3D小飞机的飞行姿态效果，并解算出欧拉角和ypr，可以直观地对比它俩的区别，如下图所示。

图7 MPU+Processing第二次联动

附录C：

MPU6050_DMP6 中有一行代码，可能会引发ESP8266不断重启，如下所示：

attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();

ESP8266不断重启的同时，串口会一直重复打印如下堆栈信息和ISR not in IRAM的报错：

Initializing I2C devices...

Testing device connections...

MPU6050 connection successful

Send any character to begin DMP programming and demo:

Initializing DMP...

>*......>......-1632.00000, 667.00000, 3960.00000, 65.00000, 22.00000, -17.00000

Enabling DMP...

Enabling interrupt detection (Arduino external interrupt 2)...

ISR not in IRAM!

User exception (panic/abort/assert)

--------------- CUT HERE FOR EXCEPTION DECODER ---------------

Abort called

我只好把这段代码注释掉。

八，外接匿名上位机(V7)

在单片机开发领域里，什么是上位机？

上位机指的是可以直接发送操作指令的计算机或者单片机，一般提供用户操作交互界面并向用户展示反馈数据。单片机主动发送状态信息或者报警信息给上位机。只要通信协议可以建立，上位机软件可以是任意开发语言和任意平台，下位机可以是单片机。

什么是匿名上位机？

匿名指的是匿名科创，主页地址：http://anotc.com/。

这个团队公布的上位机可以完成基本收发（类似于串口调试助手）、高级收发收码（实现比较复杂的自定义接收和发送）、同时显示20条波形图（用于调试PID等）、调试无人机（可以监视无人机的飞控状态以及调试PID），最新的版本号为V7.2.5。我们可以在匿名资料汇总页面下载匿名助手、匿名上位机等软件。

一定要注意下面讲的是MPU6050模块+NodeMCU开发板与匿名上位机V7的联调，再强调一遍，是V7！一定要匹配V7飞控通信协议！不是V2.6，不是V4.5，是V7！

8.1.单片机上报灵活格式帧给上位机

第一步：单片机一侧

将MPU6050获取到的六轴参数（xyz三个方向的加速度值和xyz三个方向的陀螺仪值）组装成一帧，功能码设置为0XF1。这个0XF1对应于协议里定义的灵活格式帧的功能码ID，如下图所示。

图8 灵活格式帧说明

由于每一帧的数据长度最大可以是40，所以我们可以在这个长度范围内把一些相关的数据打包放在一起，组成本帧，如下面代码所示。

注意，此时的目标地址码为0XFF，指的是广播型输出。

void Ano_Init(void) 
{
  MyAno.Head = FRAME_HEADER;//0XAA是帧头
  MyAno.Addr = GENERAL_OUTPUT_ADDR;//地址码为0xFF，指的是无特定目标，用于数据广播型输出
  MyAno.Lenth = 0;
}
//-----------------与匿名上位机通讯-----------------------------
/**  发送灵活格式帧，帧ID为0xF1～0xFA
  *  功能：发送加速度传感器和陀螺仪传感器数据给匿名上位机（V7）
  *  入口参数：第一个参数accel，它的三个元素是xyz三个方向的加速度值
  *           第二个参数gyro，它的三个元素是xyz三个方向的陀螺仪值     
  *  返回值：无
  *  注：数据格式：帧头0xAA+目标地址0xFF+功能码0xF1+数据长度LEN+DATA+和校验SC+附加校验AC
  */
void mpu6050_send_data2ano(VectorInt16 *accel,VectorInt16 *gyro)
{ 
  uint8_t nul = 0;
  Ano_Set_Mdata(0xF1,(int16_t*)&accel->x,sizeof(accel->x),1);
  Ano_Set_Mdata(0xF1,(int16_t*)&accel->y,sizeof(accel->y),2);
  Ano_Set_Mdata(0xF1,(int16_t*)&accel->z,sizeof(accel->z),3);
  Ano_Set_Mdata(0xF1,(int16_t*)&gyro->x,sizeof(gyro->x),4);
  Ano_Set_Mdata(0xF1,(int16_t*)&gyro->y,sizeof(gyro->y),5);
  Ano_Set_Mdata(0xF1,(int16_t*)&gyro->z,sizeof(gyro->z),6);
  Ano_Set_Mdata(0xF1,(uint8_t*)&nul,sizeof(nul),7);//加载数据到对应的数据位
  Ano_SendMdata();//发送数据
}

第二步：上位机一侧

上位机根据指定的波特率打开相应的串口连接，如下图所示：

图9 上位机连接设置

第三步，打开上位机的“协议解析”界面，这时候应该已经收到了来自于MPU6050的上报信息。如下图所示，注意看ID这一列，全都是F1。我们将点击右下角的设置，来使能F1帧，并自定义用户数据。

图10 上位机协议解析界面

第四步，首先，我们在“高级收码设置”的“自定义帧数据配置”里勾选F1这一行的“使能该帧”，如下图所示：

图11 上位机自定义帧数据设置

其次，我们继续在“高级收码设置”的“数据容器配置”里，依次修改容器的名称、用户帧和数据位。

如下图所示，因为我们在F1帧里传入了六轴参数，所以用户帧都选F1，数据位依次是1到6。为了方便显示和理解，我们把容器名称也都改一改。

图12 上位机数据容器设置

第五步，前面我们准备好了数据容器，就可以打开上位机的“波形分析”界面。默认它展示的是飞控基本波形的数据容器列表，没有我们的。所以我们需要打开“设置”界面，点击“用户数据波形”，然后点击“确定”，如下图所示。

图13 上位机波形设置

到此，上位机配置完成。只需要我们的单片机按照灵活格式帧的协议格式将数据发送至上位机，即可观察到自定义数据容器对应的数据值开始刷新，并绘制对应数据波形。

现在回到“波形显示”界面，左侧“波形选择”区域已经展示了我们刚才定义的六个数据容器，全部勾选之后，就可以看到右侧区域开始绘制波形了。波形显示的本质就是在一个固定的时间间隔内建立坐标然后进行连线，对于上位机的波形来说，这个时间取决于单片机发数据的频率。

上下左右摇动我们的MPU6050模块，就可以看到波形有一些大幅度的变化。

图14 上位机波形显示

到此，NodeMCU+MPU6050如何向匿名上位机（V7）发送串口数据，上位机如何配置使能该帧和数据容器，如何配置波形显示，都演示完毕了。

8.2.单片机上报飞行状态数据给上位机

毕竟如果MPU6050模块是一个无人机，还是要看到无人机模型的动画展示才更直观，下面讲一下配置流程。

第一步：单片机一侧

上位机的飞控状态至少需要单片机上报两种数据帧。

第一种是惯性传感器数据，功能码为0X01。我们把六轴参数逐一压进去即可，震动状态可以暂时置为零，如下图所示。

图15 上位机惯性数据说明

第二种是欧拉角格式数据，功能码为0X03。我们把算出来的角度乘以100，再压进去，上位机在展示的时候会自己除以100，如下图所示。

图16 上位机欧拉角数据说明

这么做的原因是为了提高数据传输的效率，当有浮点数类型数据需要传输时，根据数据类型的特点，适当截取小数点后固定几位，比如保留欧拉角的小数点后两位即可（即乘以100），将浮点数转化成整数类型进行传输，可缩短数据长度，并且避免浮点数传输时发生异常，解析成非法浮点数。

此时的目标地址码仍然是0XFF（广播型输出）。

好，现在飞控相关的基本信息类帧就组装完毕了，如下面的代码所示：

//-----------------与匿名上位机通讯-----------------------------
/**  发送基本信息类帧，功能码ID为0x01～0x04之间，在飞控通信协议里属于飞控相关信息类
  *  功能：发送惯性传感器、欧拉角等数据给匿名上位机（V7）
  *  入口参数：第一个参数accel，它的三个元素是xyz三个方向的加速度值
  *           第二个参数gyro，它的三个元素是xyz三个方向的陀螺仪值
  *           第三个参数ypr，这是一个float[]数组，分别是yaw/pitch/roll angles原始值
  *  返回值：无
  *  注：数据格式：帧头0xAA+目标地址0xFF+功能码+数据长度LEN+DATA+和校验SC+附加校验AC
  *  注：为了提高数据传输的效率，当有浮点数类型数据需要传输时，根据数据类型的特点，适当截取小数点后固定几
        位，比如飞控姿态数据，保留角度的小数点后两位即可（即乘以100）。
        将浮点数转化成整数类型进行传输，可缩短数据长度，并且避免浮点数传输时发生异
        常，解析成非法浮点数。
  */
void mpu6050_send_flydata2ano(VectorInt16 *accel,VectorInt16 *gyro,float *ypr)
{ 
  uint8_t nul = 0;
  //ID：0x01：惯性传感器数据
  //DATA 区域内容：
  //ACC、GYR：依次为加速度、陀螺仪传感器数据。
  //SHOCK_STA：震动状态
  Ano_Set_Mdata(0x01,(int16_t*)&accel->x,sizeof(accel->x),1);
  Ano_Set_Mdata(0x01,(int16_t*)&accel->y,sizeof(accel->y),2);
  Ano_Set_Mdata(0x01,(int16_t*)&accel->z,sizeof(accel->z),3);
  Ano_Set_Mdata(0x01,(int16_t*)&gyro->x,sizeof(gyro->x),4);
  Ano_Set_Mdata(0x01,(int16_t*)&gyro->y,sizeof(gyro->y),5);
  Ano_Set_Mdata(0x01,(int16_t*)&gyro->z,sizeof(gyro->z),6);
  Ano_Set_Mdata(0x01,(uint8_t*)&nul,sizeof(nul),7);
  Ano_SendMdata();//发送数据
  //ID：0x03：飞控姿态：欧拉角格式
  //DATA 区域内容：
  //ROL、PIT、YAW：姿态角，依次为横滚、俯仰、航向，精确到 0.01。
  //FUSION _STA：融合状态
  //注意：角度最终要乘以100，上位机那边会除以100做展示
  int16_t rol = (int16_t)(ypr[2] * 180 * 100.00 / M_PI);
  int16_t pit = (int16_t)(ypr[1] * 180 * 100.00 / M_PI);
  int16_t yaw = (int16_t)(ypr[0] * 180 * 100.00 / M_PI);
  Ano_Set_Mdata(0x03,(int16_t*)&rol,sizeof(rol),1);
  Ano_Set_Mdata(0x03,(int16_t*)&pit,sizeof(pit),2);
  Ano_Set_Mdata(0x03,(int16_t*)&yaw,sizeof(yaw),3);
  Ano_Set_Mdata(0x03,(uint8_t*)&nul,sizeof(nul),4);
  Ano_SendMdata();//发送数据
}

第二步，上位机一侧

在上位机里打开连接后，我们就会发现已经有数据进来了。打开“飞行状态”界面（如下图所示），点击下方的“数据显示”按钮。

图17 上位机飞行状态界面

我们就会看到飞控通信协议里的基本信息类帧都在这里罗列了，如下图所示。我们点开单片机本次上报的“Frame ID: 0x01”和“Frame ID: 0x03”，就会看到两个要点：

第一，数据值这一列一直在变化，说明数据都能被正确地解析；

第二，传输缩放这一列里，惯性传感器数据都没有做缩放，而欧拉角则做了100（即1.0E+2）的倍数缩放。

图18 上位机数据显示界面

第三步，我们打开上位机的“波形显示”画面，在它的设置项里选“飞控基本波形”并确定，就会看到波形显示的左侧区域展示了惯性传感器和欧拉角等预定义数据项，勾选之后就会看到波形在实时变化，随着你摇动旋转MPU6050模块。

图19 上位机波形设置

第四步，MPU6050与上位机飞行状态联动的效果如下图所示：

图20 单片机与上位机飞行姿态联动

到此，单片机按照匿名公司的飞控通信协议V7（注意版本号哦），将MPU6050模块启用自带的DMP计算出来的欧拉角等数据，以基本信息类帧的数据格式，通过串口上报给电脑端的匿名上位机（V7），从而可以让MPU6050的运动姿态联动上位机的无人机模型。

最后，我们总结一下：

0x01：【元器件基础】学习了如何基于Adafruit_MPU6050和Kalman_Filter_Library库，利用MPU6050模块，获得六轴参数，并进一步计算俯仰角和横滚角。

0x02：【元器件进阶】学习MPU6050如何对接Processing IDE，在电脑端实时绘制模型的飞行姿态。

0x03：【元器件进阶】学习MPU6050如何对接匿名上位机（V7），在上位机里收码、波形显示和实时飞行姿态。

-END-

posted @ 2023-04-04 15:13 老兵笔记阅读(1690) 评论(0) 编辑收藏举报

刷新页面返回顶部