基于ubuntu环境，搭建Arduino+ESP32+MPU6050验证系统，并利用Processing仿真

具体姿态解算和原理性验证，姿态反馈控制另文描述，本文只针对基本验证环境搭建。

1. 建立基于Arduino的ESP32开发环境

原来用micro和promini都做了环境，但是隔了一段时间居然串口烧录把板子搞挂了，pro mini串口烧录也老出问题，先放一放，手上刚好有EPS32的开发板，毕竟ESP32带了wifi和蓝牙，也有多个串口，资源够用，也方便数据传输，用来读取MPU6050开发板的数据做原理性验证绰绰有余了。

Arduino直接下ESP32的包

GitHub - pocean2001/arduino-esp32: Arduino core for the ESP32

在Arduino下建立hardware/espressif目录，下载后解压，可以把目录名改为esp32

记得要运行工具

(base) hy@hy-Mi-Gaming-Laptop-15-6:~/arduino-1.8.13/hardware/espressif/arduino-esp32/tools$ python get.py
System: Linux, Bits: 64, Info: Linux-5.4.0-65-generic-x86_64-with-glibc2.10
Platform: x86_64-pc-linux-gnu
Downloading xtensa-esp32-elf-linux64-1.22.0-80-g6c4433a-5.2.0.tar.gz ...
Done
Extracting xtensa-esp32-elf-linux64-1.22.0-80-g6c4433a-5.2.0.tar.gz ...
Downloading esptool-2.6.1-linux.tar.gz ...
Done
Extracting esptool-2.6.1-linux.tar.gz ...
Downloading mkspiffs-0.2.3-arduino-esp32-linux64.tar.gz ...
Done
Extracting mkspiffs-0.2.3-arduino-esp32-linux64.tar.gz ...
Renaming mkspiffs-0.2.3-arduino-esp32-linux64 to mkspiffs ...
Platform Tools Installed

打开Arduino，选择板子和端口，此时需要把串口的权限开一下

sudo chmod 777 /dev/ttyUSB0

2. 下载MPU6050的开发库

能下的都搞下来玩一遍

3. 修改接口，测试传感器数据输出

用的esp32的引脚图

例子MPU6050_DMP6有多种数据输出形式，就用它，要改几个地方。

用wire.h库做驱动

 #include "Wire.h"

定义中断引脚

#define INTERRUPT_PIN 5

定义I2C接口引脚scl/sda

 Wire.begin(4,15);

这一段宏定义定义数据输出方式：

// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
// quaternion components in a [w, x, y, z] format (not best for parsing
// on a remote host such as Processing or something though)
#define OUTPUT_READABLE_QUATERNION

// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
// (in degrees) calculated from the quaternions coming from the FIFO.
// Note that Euler angles suffer from gimbal lock (for more info, see
// http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_EULER

// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
// pitch/roll angles (in degrees) calculated from the quaternions coming
// from the FIFO. Note this also requires gravity vector calculations.
// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
//#define OUTPUT_READABLE_YAWPITCHROLL

// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
// components with gravity removed. This acceleration reference frame is
// not compensated for orientation, so +X is always +X according to the
// sensor, just without the effects of gravity. If you want acceleration
// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
//#define OUTPUT_READABLE_REALACCEL

// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
// components with gravity removed and adjusted for the world frame of
// reference (yaw is relative to initial orientation, since no magnetometer
// is present in this case). Could be quite handy in some cases.
//#define OUTPUT_READABLE_WORLDACCEL

// uncomment "OUTPUT_TEAPOT" if you want output that matches the
// format used for the InvenSense teapot demo
//#define OUTPUT_TEAPOT

对应下面的数据处理输出：

 #ifdef OUTPUT_READABLE_QUATERNION
            // display quaternion values in easy matrix form: w x y z
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            Serial.print("quat\t");
            Serial.print(q.w);
            Serial.print("\t");
            Serial.print(q.x);
            Serial.print("\t");
            Serial.print(q.y);
            Serial.print("\t");
            Serial.println(q.z);
        #endif

        #ifdef OUTPUT_READABLE_EULER
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetEuler(euler, &q);
            Serial.print("euler\t");
            Serial.print(euler[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(euler[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(euler[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_YAWPITCHROLL
            // display Euler angles in degrees
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
            Serial.print("ypr\t");
            Serial.print(ypr[0] * 180/M_PI);
            Serial.print("\t");
            Serial.print(ypr[1] * 180/M_PI);
            Serial.print("\t");
            Serial.println(ypr[2] * 180/M_PI);
        #endif

        #ifdef OUTPUT_READABLE_REALACCEL
            // display real acceleration, adjusted to remove gravity
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            Serial.print("areal\t");
            Serial.print(aaReal.x);
            Serial.print("\t");
            Serial.print(aaReal.y);
            Serial.print("\t");
            Serial.println(aaReal.z);
        #endif

        #ifdef OUTPUT_READABLE_WORLDACCEL
            // display initial world-frame acceleration, adjusted to remove gravity
            // and rotated based on known orientation from quaternion
            mpu.dmpGetQuaternion(&q, fifoBuffer);
            mpu.dmpGetAccel(&aa, fifoBuffer);
            mpu.dmpGetGravity(&gravity, &q);
            mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
            mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
            Serial.print("aworld\t");
            Serial.print(aaWorld.x);