【ESP32 Arduino平衡小车制作】（二) mpu6050 dmp欧拉角解算 PlatformIO

mpu6050 dmp6 欧拉角解算

MPU6050一款常用的集成加速度计的陀螺仪。今天就来介绍ESP32 Arduino如何使用mpu6050做dmp ，四元数姿态解算，从而获得欧拉角和xyz角。
最后的效果：可以看到很好的实验效果，eular和ypr角。
此外我使用的是esp32 PICO4芯片，PlatformIO开发环境 mpu6050接PIN32,PIN 33引脚

一、库安装：

用过PlatformIO的朋友们肯定知道，PlatformIO具有丰富的库文件，使用来那是十分的方便。在库中搜素mpu6050的话，会出现两个库。

Adafruit MPU6050 库：直接读取数据，不能进行dmp解算

MPU6050库：可以进行dmp解算，但是下载之后不可以用，文件不全。 编译的时候程序报错。

为此我找了好久，最后在github上找到了最新的库，这里我将库分享到百度网盘，需要自取。链接：https://pan.baidu.com/s/14h430aUozn4PEDeSu13n3Q 提取码：nmi6

内含一个可以使用的工程，和mpu6050的库

二、程序编写：

定义相关的宏之后就会显示不同的数据

#define OUTPUT_READABLE_QUATERNION  // 显示四元数值
#define OUTPUT_READABLE_EULER		// 以度为单位显示欧拉角度
#define OUTPUT_READABLE_YAWPITCHROLL// 以度为单位显示欧拉角度
#define OUTPUT_READABLE_REALACCEL	// 显示真实加速度，调整以消除重力 
#define OUTPUT_READABLE_WORLDACCEL	// 显示初始世界帧加速度，调整以移除重力，并基于四元数的已知方向旋转
#define OUTPUT_TEAPOT				// 显示四元数值

#include <Arduino.h>
#include <I2Cdev.h>
#include <MPU6050_6Axis_MotionApps20.h>
#include "Wire.h"

MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high

#define OUTPUT_READABLE_YAWPITCHROLL
#define OUTPUT_READABLE_EULER
#define LED_PIN 27 
bool blinkState = false;

// MPU control/status vars
bool dmpReady = false;  // set true if DMP init was successful
uint8_t mpuIntStatus;   // holds actual interrupt status byte from MPU
uint8_t devStatus;      // return status after each device operation (0 = success, !0 = error)
uint16_t packetSize;    // expected DMP packet size (default is 42 bytes)
uint16_t fifoCount;     // count of all bytes currently in FIFO
uint8_t fifoBuffer[64]; // FIFO storage buffer

// orientation/motion vars
Quaternion q;           // [w, x, y, z]         quaternion container
VectorInt16 aa;         // [x, y, z]            accel sensor measurements
VectorInt16 aaReal;     // [x, y, z]            gravity-free accel sensor measurements
VectorInt16 aaWorld;    // [x, y, z]            world-frame accel sensor measurements
VectorFloat gravity;    // [x, y, z]            gravity vector
float euler[3];         // [psi, theta, phi]    Euler angle container
float ypr[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

// packet structure for InvenSense teapot demo
uint8_t teapotPacket[14] = {
    '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };

// ================================================================
// ===               INTERRUPT DETECTION ROUTINE                ===
// =======================================================&