【ESP32Arduino+MPU6050 dmp姿态解算】学习笔记 PlatformIO 复制即可使用

最新推荐文章于 2024-09-24 17:56:53 发布
最新推荐文章于 2024-09-24 17:56:53 发布
阅读量1.9w 收藏 82
点赞数 10
文章标签： fpga开发
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
本文链接：https://blog.csdn.net/colour64/article/details/126321158
版权

0.引入

 esp32，国产之光，拥有先进的网络功能和强大的双核处理器；mpu6050，经典的六轴陀螺仪，体积小价格低廉但是功能强大。

 二者的优点相结合，可实现对机体高效的姿态测算以及以此进行远程控制。

相信同学们在学习使用esp32读取mpu6050数据时都会遇到如下问题：

1.网上下载下来的程序报错，无法运行，主要是板子不匹配

2.网上关于esp32的资料太少，找不到直接能用的程序和教程

3.基础程序只能读取出原始数据，但是极不整齐，不直观，且杂波较多，数据偏移较大。

1.实现过程

器材：

1.esp32开发板（本人使用的是esp wroom 32开发板，这个用其它的esp系列应该都可以）

2.MPU6050陀螺仪模块

3.VScode以及PlatformIO

（PlatfoIO具体环境配置见此视频：Arduino IDE太难用？5分钟"离线"安装PlatformIO，无需等待，编程体验原地起飞_哔哩哔哩_bilibili）

4.数据线、杜邦线、面包板等

若想实现直接输出经过dmp解算的数据，即直接输出Pitch、Roll、Yaw以及加上平动三个维度共六个自由度的姿态数据，需要较为复杂的对mpu6050的初始化过程。其实这一过程，本人也是一知半解，毕竟本人也是小白，并非专业人士。

经过多次测试后本人发现，arduinoIDE中MPU6050库的示例程序，在arduino开发板上即可运行，输出稳定且精准。其经过一些改动后可移植到esp32上运行，且效果相同！

 

该示例中的程序直接导入esp32中无法运行，会卡在初始化无法继续。经过本人不断排查，发现此程序包含的I2C库中，对于mpu6050寄存器的数据读写操作程序esp32无法执行。i2cdevlib只支持arduino nano等系列芯片，以及esp8266，但不支持esp32。这是由于Wire库的读写操作在esp32和其它开发板之间有微小的差别，如下图：

首先是arduino示例原码：

for (int k = 0; k < length; k += min((int)length, I2CDEVLIB_WIRE_BUFFER_LENGTH)) {
    useWire->beginTransmission(devAddr);
    useWire->write(regAddr);
    useWire->endTransmission();           
    useWire->beginTransmission(devAddr);    //注意此行！！！！！
    useWire->requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));
        
    for (; useWire->available() && (timeout == 0 || millis() - t1 < timeout); count++) {
        data[count] = useWire->read();
      }
    }

修改后适用于esp32的代码：

for (int k = 0; k < length; k += min((int)length, 128)) {
    Wire.beginTransmission(devAddr);
    Wire.write(regAddr);
    Wire.endTransmission();
    Wire.requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, I2CDEVLIB_WIRE_BUFFER_LENGTH));

    for (;Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
      data[count] = Wire.read();
    }
  }

注意到，在for循环前面的代码中，arduino原码中带注释的那一行在esp32的代码中是不存在的，Wire.beginTransmission(devAddr) 这一行代码会使esp32卡在此处无法继续运行。

诸如此类的差异还有若干，在此不一一赘述。

2.干货

下面，是经过修改过的全部代码（甚至不需要安装I2C库）复制即可使用：

#include <Arduino.h>
#include <Wire.h>
#include <algorithm>

/*=================================================================================================*/

/*                                         初始化数据                                              */

/*=================================================================================================*/

#define MPU6050_DMP_CODE_SIZE       1929
static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
	/* bank # 0 */
	0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
	0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
	0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00,
	0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
	0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
	0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
	0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
	0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
	0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
	/* bank # 1 */
	0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
	0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
	0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
	0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
	0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00,
	0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
	/* bank # 2 */
	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
	/* bank # 3 */
	0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
	0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
	0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
	0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
	0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
	0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
	0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
	0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C,
	0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
	0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
	0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
	0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
	0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
	0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
	0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
	0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
	/* bank # 4 */
	0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
	0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
	0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
	0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
	0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
	0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
	0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
	0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
	0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
	0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
	0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
	0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
	0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
	0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
	/* bank # 5 */
	0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
	0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
	0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
	0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
	0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
	0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
	0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
	0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
	0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
	0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
	0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
	0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
	0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
	0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
	0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
	0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
	/* bank # 6 */
	0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
	0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
	0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
	0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
	0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
	0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
	0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
	0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
	0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
	0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
	0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
	0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
	0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
	0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
	0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
	0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
	/* bank # 7 */
	0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
	0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
	0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
	0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
	0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
	0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
	0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38,
	0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC,
	0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF,

};


#define mpu_addr 0x68
uint8_t buffer[14];

#define MPU6050_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
#define MPU6050_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
#define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW

#define MPU6050_RA_XG_OFFS_TC       0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_YG_OFFS_TC       0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_ZG_OFFS_TC       0x02 //[7] PWR_MODE, [6:1] ZG_OFFS_TC, [0] OTP_BNK_VLD
#define MPU6050_RA_X_FINE_GAIN      0x03 //[7:0] X_FINE_GAIN
#define MPU6050_RA_Y_FINE_GAIN      0x04 //[7:0] Y_FINE_GAIN
#define MPU6050_RA_Z_FINE_GAIN      0x05 //[7:0] Z_FINE_GAIN
#define MPU6050_RA_XA_OFFS_H        0x06 //[15:0] XA_OFFS
#define MPU6050_RA_XA_OFFS_L_TC     0x07
#define MPU6050_RA_YA_OFFS_H        0x08 //[15:0] YA_OFFS
#define MPU6050_RA_YA_OFFS_L_TC     0x09
#define MPU6050_RA_ZA_OFFS_H        0x0A //[15:0] ZA_OFFS
#define MPU6050_RA_ZA_OFFS_L_TC     0x0B
#define MPU6050_RA_SELF_TEST_X      0x0D //[7:5] XA_TEST[4-2], [4:0] XG_TEST[4-0]
#define MPU6050_RA_SELF_TEST_Y      0x0E //[7:5] YA_TEST[4-2], [4:0] YG_TEST[4-0]
#define MPU6050_RA_SELF_TEST_Z      0x0F //[7:5] ZA_TEST[4-2], [4:0] ZG_TEST[4-0]
#define MPU6050_RA_SELF_TEST_A      0x10 //[5:4] XA_TEST[1-0], [3:2] YA_TEST[1-0], [1:0] ZA_TEST[1-0]
#define MPU6050_RA_XG_OFFS_USRH     0x13 //[15:0] XG_OFFS_USR
#define MPU6050_RA_XG_OFFS_USRL     0x14
#define MPU6050_RA_YG_OFFS_USRH     0x15 //[15:0] YG_OFFS_USR
#define MPU6050_RA_YG_OFFS_USRL     0x16
#define MPU6050_RA_ZG_OFFS_USRH     0x17 //[15:0] ZG_OFFS_USR
#define MPU6050_RA_ZG_OFFS_USRL     0x18
#define MPU6050_RA_SMPLRT_DIV       0x19
#define MPU6050_RA_CONFIG           0x1A
#define MPU6050_RA_GYRO_CONFIG      0x1B
#define MPU6050_RA_ACCEL_CONFIG     0x1C
#define MPU6050_RA_FF_THR           0x1D
#define MPU6050_RA_FF_DUR           0x1E
#define MPU6050_RA_MOT_THR          0x1F
#define MPU6050_RA_MOT_DUR          0x20
#define MPU6050_RA_ZRMOT_THR        0x21
#define MPU6050_RA_ZRMOT_DUR        0x22
#define MPU6050_RA_FIFO_EN          0x23
#define MPU6050_RA_I2C_MST_CTRL     0x24
#define MPU6050_RA_I2C_SLV0_ADDR    0x25
#define MPU6050_RA_I2C_SLV0_REG     0x26
#define MPU6050_RA_I2C_SLV0_CTRL    0x27
#define MPU6050_RA_I2C_SLV1_ADDR    0x28
#define MPU6050_RA_I2C_SLV1_REG     0x29
#define MPU6050_RA_I2C_SLV1_CTRL    0x2A
#define MPU6050_RA_I2C_SLV2_ADDR    0x2B
#define MPU6050_RA_I2C_SLV2_REG     0x2C
#define MPU6050_RA_I2C_SLV2_CTRL    0x2D
#define MPU6050_RA_I2C_SLV3_ADDR    0x2E
#define MPU6050_RA_I2C_SLV3_REG     0x2F
#define MPU6050_RA_I2C_SLV3_CTRL    0x30
#define MPU6050_RA_I2C_SLV4_ADDR    0x31
#define MPU6050_RA_I2C_SLV4_REG     0x32
#define MPU6050_RA_I2C_SLV4_DO      0x33
#define MPU6050_RA_I2C_SLV4_CTRL    0x34
#define MPU6050_RA_I2C_SLV4_DI      0x35
#define MPU6050_RA_I2C_MST_STATUS   0x36
#define MPU6050_RA_INT_PIN_CFG      0x37
#define MPU6050_RA_INT_ENABLE       0x38
#define MPU6050_RA_DMP_INT_STATUS   0x39
#define MPU6050_RA_INT_STATUS       0x3A
#define MPU6050_RA_ACCEL_XOUT_H     0x3B
#define MPU6050_RA_ACCEL_XOUT_L     0x3C
#define MPU6050_RA_ACCEL_YOUT_H     0x3D
#define MPU6050_RA_ACCEL_YOUT_L     0x3E
#define MPU6050_RA_ACCEL_ZOUT_H     0x3F
#define MPU6050_RA_ACCEL_ZOUT_L     0x40
#define MPU6050_RA_TEMP_OUT_H       0x41
#define MPU6050_RA_TEMP_OUT_L       0x42
#define MPU6050_RA_GYRO_XOUT_H      0x43
#define MPU6050_RA_GYRO_XOUT_L      0x44
#define MPU6050_RA_GYRO_YOUT_H      0x45
#define MPU6050_RA_GYRO_YOUT_L      0x46
#define MPU6050_RA_GYRO_ZOUT_H      0x47
#define MPU6050_RA_GYRO_ZOUT_L      0x48
#define MPU6050_RA_EXT_SENS_DATA_00 0x49
#define MPU6050_RA_EXT_SENS_DATA_01 0x4A
#define MPU6050_RA_EXT_SENS_DATA_02 0x4B
#define MPU6050_RA_EXT_SENS_DATA_03 0x4C
#define MPU6050_RA_EXT_SENS_DATA_04 0x4D
#define MPU6050_RA_EXT_SENS_DATA_05 0x4E
#define MPU6050_RA_EXT_SENS_DATA_06 0x4F
#define MPU6050_RA_EXT_SENS_DATA_07 0x50
#define MPU6050_RA_EXT_SENS_DATA_08 0x51
#define MPU6050_RA_EXT_SENS_DATA_09 0x52
#define MPU6050_RA_EXT_SENS_DATA_10 0x53
#define MPU6050_RA_EXT_SENS_DATA_11 0x54
#define MPU6050_RA_EXT_SENS_DATA_12 0x55
#define MPU6050_RA_EXT_SENS_DATA_13 0x56
#define MPU6050_RA_EXT_SENS_DATA_14 0x57
#define MPU6050_RA_EXT_SENS_DATA_15 0x58
#define MPU6050_RA_EXT_SENS_DATA_16 0x59
#define MPU6050_RA_EXT_SENS_DATA_17 0x5A
#define MPU6050_RA_EXT_SENS_DATA_18 0x5B
#define MPU6050_RA_EXT_SENS_DATA_19 0x5C
#define MPU6050_RA_EXT_SENS_DATA_20 0x5D
#define MPU6050_RA_EXT_SENS_DATA_21 0x5E
#define MPU6050_RA_EXT_SENS_DATA_22 0x5F
#define MPU6050_RA_EXT_SENS_DATA_23 0x60
#define MPU6050_RA_MOT_DETECT_STATUS    0x61
#define MPU6050_RA_I2C_SLV0_DO      0x63
#define MPU6050_RA_I2C_SLV1_DO      0x64
#define MPU6050_RA_I2C_SLV2_DO      0x65
#define MPU6050_RA_I2C_SLV3_DO      0x66
#define MPU6050_RA_I2C_MST_DELAY_CTRL   0x67
#define MPU6050_RA_SIGNAL_PATH_RESET    0x68
#define MPU6050_RA_MOT_DETECT_CTRL      0x69
#define MPU6050_RA_USER_CTRL        0x6A
#define MPU6050_RA_PWR_MGMT_1       0x6B
#define MPU6050_RA_PWR_MGMT_2       0x6C
#define MPU6050_RA_BANK_SEL         0x6D
#define MPU6050_RA_MEM_START_ADDR   0x6E
#define MPU6050_RA_MEM_R_W          0x6F
#define MPU6050_RA_DMP_CFG_1        0x70
#define MPU6050_RA_DMP_CFG_2        0x71
#define MPU6050_RA_FIFO_COUNTH      0x72
#define MPU6050_RA_FIFO_COUNTL      0x73
#define MPU6050_RA_FIFO_R_W         0x74
#define MPU6050_RA_WHO_AM_I         0x75

#define MPU6050_SELF_TEST_XA_1_BIT     0x07
#define MPU6050_SELF_TEST_XA_1_LENGTH  0x03
#define MPU6050_SELF_TEST_XA_2_BIT     0x05
#define MPU6050_SELF_TEST_XA_2_LENGTH  0x02
#define MPU6050_SELF_TEST_YA_1_BIT     0x07
#define MPU6050_SELF_TEST_YA_1_LENGTH  0x03
#define MPU6050_SELF_TEST_YA_2_BIT     0x03
#define MPU6050_SELF_TEST_YA_2_LENGTH  0x02
#define MPU6050_SELF_TEST_ZA_1_BIT     0x07
#define MPU6050_SELF_TEST_ZA_1_LENGTH  0x03
#define MPU6050_SELF_TEST_ZA_2_BIT     0x01
#define MPU6050_SELF_TEST_ZA_2_LENGTH  0x02

#define MPU6050_SELF_TEST_XG_1_BIT     0x04
#define MPU6050_SELF_TEST_XG_1_LENGTH  0x05
#define MPU6050_SELF_TEST_YG_1_BIT     0x04
#define MPU6050_SELF_TEST_YG_1_LENGTH  0x05
#define MPU6050_SELF_TEST_ZG_1_BIT     0x04
#define MPU6050_SELF_TEST_ZG_1_LENGTH  0x05

#define MPU6050_TC_PWR_MODE_BIT     7
#define MPU6050_TC_OFFSET_BIT       6
#define MPU6050_TC_OFFSET_LENGTH    6
#define MPU6050_TC_OTP_BNK_VLD_BIT  0

#define MPU6050_VDDIO_LEVEL_VLOGIC  0
#define MPU6050_VDDIO_LEVEL_VDD     1

#define MPU6050_CFG_EXT_SYNC_SET_BIT    5
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
#define MPU6050_CFG_DLPF_CFG_BIT    2
#define MPU6050_CFG_DLPF_CFG_LENGTH 3

#define MPU6050_EXT_SYNC_DISABLED       0x0
#define MPU6050_EXT_SYNC_TEMP_OUT_L     0x1
#define MPU6050_EXT_SYNC_GYRO_XOUT_L    0x2
#define MPU6050_EXT_SYNC_GYRO_YOUT_L    0x3
#define MPU6050_EXT_SYNC_GYRO_ZOUT_L    0x4
#define MPU6050_EXT_SYNC_ACCEL_XOUT_L   0x5
#define MPU6050_EXT_SYNC_ACCEL_YOUT_L   0x6
#define MPU6050_EXT_SYNC_ACCEL_ZOUT_L   0x7

#define MPU6050_DLPF_BW_256         0x00
#define MPU6050_DLPF_BW_188         0x01
#define MPU6050_DLPF_BW_98          0x02
#define MPU6050_DLPF_BW_42          0x03
#define MPU6050_DLPF_BW_20          0x04
#define MPU6050_DLPF_BW_10          0x05
#define MPU6050_DLPF_BW_5           0x06

#define MPU6050_GCONFIG_FS_SEL_BIT      4
#define MPU6050_GCONFIG_FS_SEL_LENGTH   2

#define MPU6050_GYRO_FS_250         0x00
#define MPU6050_GYRO_FS_500         0x01
#define MPU6050_GYRO_FS_1000        0x02
#define MPU6050_GYRO_FS_2000        0x03

#define MPU6050_ACONFIG_XA_ST_BIT           7
#define MPU6050_ACONFIG_YA_ST_BIT           6
#define MPU6050_ACONFIG_ZA_ST_BIT           5
#define MPU6050_ACONFIG_AFS_SEL_BIT         4
#define MPU6050_ACONFIG_AFS_SEL_LENGTH      2
#define MPU6050_ACONFIG_ACCEL_HPF_BIT       2
#define MPU6050_ACONFIG_ACCEL_HPF_LENGTH    3

#define MPU6050_ACCEL_FS_2          0x00
#define MPU6050_ACCEL_FS_4          0x01
#define MPU6050_ACCEL_FS_8          0x02
#define MPU6050_ACCEL_FS_16         0x03

#define MPU6050_DHPF_RESET          0x00
#define MPU6050_DHPF_5              0x01
#define MPU6050_DHPF_2P5            0x02
#define MPU6050_DHPF_1P25           0x03
#define MPU6050_DHPF_0P63           0x04
#define MPU6050_DHPF_HOLD           0x07

#define MPU6050_TEMP_FIFO_EN_BIT    7
#define MPU6050_XG_FIFO_EN_BIT      6
#define MPU6050_YG_FIFO_EN_BIT      5
#define MPU6050_ZG_FIFO_EN_BIT      4
#define MPU6050_ACCEL_FIFO_EN_BIT   3
#define MPU6050_SLV2_FIFO_EN_BIT    2
#define MPU6050_SLV1_FIFO_EN_BIT    1
#define MPU6050_SLV0_FIFO_EN_BIT    0

#define MPU6050_MULT_MST_EN_BIT     7
#define MPU6050_WAIT_FOR_ES_BIT     6
#define MPU6050_SLV_3_FIFO_EN_BIT   5
#define MPU6050_I2C_MST_P_NSR_BIT   4
#define MPU6050_I2C_MST_CLK_BIT     3
#define MPU6050_I2C_MST_CLK_LENGTH  4

#define MPU6050_CLOCK_DIV_348       0x0
#define MPU6050_CLOCK_DIV_333       0x1
#define MPU6050_CLOCK_DIV_320       0x2
#define MPU6050_CLOCK_DIV_308       0x3
#define MPU6050_CLOCK_DIV_296       0x4
#define MPU6050_CLOCK_DIV_286       0x5
#define MPU6050_CLOCK_DIV_276       0x6
#define MPU6050_CLOCK_DIV_267       0x7
#define MPU6050_CLOCK_DIV_258       0x8
#define MPU6050_CLOCK_DIV_500       0x9
#define MPU6050_CLOCK_DIV_471       0xA
#define MPU6050_CLOCK_DIV_444       0xB
#define MPU6050_CLOCK_DIV_421       0xC
#define MPU6050_CLOCK_DIV_400       0xD
#define MPU6050_CLOCK_DIV_381       0xE
#define MPU6050_CLOCK_DIV_364       0xF

#define MPU6050_I2C_SLV_RW_BIT      7
#define MPU6050_I2C_SLV_ADDR_BIT    6
#define MPU6050_I2C_SLV_ADDR_LENGTH 7
#define MPU6050_I2C_SLV_EN_BIT      7
#define MPU6050_I2C_SLV_BYTE_SW_BIT 6
#define MPU6050_I2C_SLV_REG_DIS_BIT 5
#define MPU6050_I2C_SLV_GRP_BIT     4
#define MPU6050_I2C_SLV_LEN_BIT     3
#define MPU6050_I2C_SLV_LEN_LENGTH  4

#define MPU6050_I2C_SLV4_RW_BIT         7
#define MPU6050_I2C_SLV4_ADDR_BIT       6
#define MPU6050_I2C_SLV4_ADDR_LENGTH    7
#define MPU6050_I2C_SLV4_EN_BIT         7
#define MPU6050_I2C_SLV4_INT_EN_BIT     6
#define MPU6050_I2C_SLV4_REG_DIS_BIT    5
#define MPU6050_I2C_SLV4_MST_DLY_BIT    4
#define MPU6050_I2C_SLV4_MST_DLY_LENGTH 5

#define MPU6050_MST_PASS_THROUGH_BIT    7
#define MPU6050_MST_I2C_SLV4_DONE_BIT   6
#define MPU6050_MST_I2C_LOST_ARB_BIT    5
#define MPU6050_MST_I2C_SLV4_NACK_BIT   4
#define MPU6050_MST_I2C_SLV3_NACK_BIT   3
#define MPU6050_MST_I2C_SLV2_NACK_BIT   2
#define MPU6050_MST_I2C_SLV1_NACK_BIT   1
#define MPU6050_MST_I2C_SLV0_NACK_BIT   0

#define MPU6050_INTCFG_INT_LEVEL_BIT        7
#define MPU6050_INTCFG_INT_OPEN_BIT         6
#define MPU6050_INTCFG_LATCH_INT_EN_BIT     5
#define MPU6050_INTCFG_INT_RD_CLEAR_BIT     4
#define MPU6050_INTCFG_FSYNC_INT_LEVEL_BIT  3
#define MPU6050_INTCFG_FSYNC_INT_EN_BIT     2
#define MPU6050_INTCFG_I2C_BYPASS_EN_BIT    1
#define MPU6050_INTCFG_CLKOUT_EN_BIT        0

#define MPU6050_INTMODE_ACTIVEHIGH  0x00
#define MPU6050_INTMODE_ACTIVELOW   0x01

#define MPU6050_INTDRV_PUSHPULL     0x00
#define MPU6050_INTDRV_OPENDRAIN    0x01

#define MPU6050_INTLATCH_50USPULSE  0x00
#define MPU6050_INTLATCH_WAITCLEAR  0x01

#define MPU6050_INTCLEAR_STATUSREAD 0x00
#define MPU6050_INTCLEAR_ANYREAD    0x01

#define MPU6050_INTERRUPT_FF_BIT            7
#define MPU6050_INTERRUPT_MOT_BIT           6
#define MPU6050_INTERRUPT_ZMOT_BIT          5
#define MPU6050_INTERRUPT_FIFO_OFLOW_BIT    4
#define MPU6050_INTERRUPT_I2C_MST_INT_BIT   3
#define MPU6050_INTERRUPT_PLL_RDY_INT_BIT   2
#define MPU6050_INTERRUPT_DMP_INT_BIT       1
#define MPU6050_INTERRUPT_DATA_RDY_BIT      0

// TODO: figure out what these actually do
// UMPL source code is not very obivous
#define MPU6050_DMPINT_5_BIT            5
#define MPU6050_DMPINT_4_BIT            4
#define MPU6050_DMPINT_3_BIT            3
#define MPU6050_DMPINT_2_BIT            2
#define MPU6050_DMPINT_1_BIT            1
#define MPU6050_DMPINT_0_BIT            0

#define MPU6050_MOTION_MOT_XNEG_BIT     7
#define MPU6050_MOTION_MOT_XPOS_BIT     6
#define MPU6050_MOTION_MOT_YNEG_BIT     5
#define MPU6050_MOTION_MOT_YPOS_BIT     4
#define MPU6050_MOTION_MOT_ZNEG_BIT     3
#define MPU6050_MOTION_MOT_ZPOS_BIT     2
#define MPU6050_MOTION_MOT_ZRMOT_BIT    0

#define MPU6050_DELAYCTRL_DELAY_ES_SHADOW_BIT   7
#define MPU6050_DELAYCTRL_I2C_SLV4_DLY_EN_BIT   4
#define MPU6050_DELAYCTRL_I2C_SLV3_DLY_EN_BIT   3
#define MPU6050_DELAYCTRL_I2C_SLV2_DLY_EN_BIT   2
#define MPU6050_DELAYCTRL_I2C_SLV1_DLY_EN_BIT   1
#define MPU6050_DELAYCTRL_I2C_SLV0_DLY_EN_BIT   0

#define MPU6050_PATHRESET_GYRO_RESET_BIT    2
#define MPU6050_PATHRESET_ACCEL_RESET_BIT   1
#define MPU6050_PATHRESET_TEMP_RESET_BIT    0

#define MPU6050_DETECT_ACCEL_ON_DELAY_BIT       5
#define MPU6050_DETECT_ACCEL_ON_DELAY_LENGTH    2
#define MPU6050_DETECT_FF_COUNT_BIT             3
#define MPU6050_DETECT_FF_COUNT_LENGTH          2
#define MPU6050_DETECT_MOT_COUNT_BIT            1
#define MPU6050_DETECT_MOT_COUNT_LENGTH         2

#define MPU6050_DETECT_DECREMENT_RESET  0x0
#define MPU6050_DETECT_DECREMENT_1      0x1
#define MPU6050_DETECT_DECREMENT_2      0x2
#define MPU6050_DETECT_DECREMENT_4      0x3

#define MPU6050_USERCTRL_DMP_EN_BIT             7
#define MPU6050_USERCTRL_FIFO_EN_BIT            6
#define MPU6050_USERCTRL_I2C_MST_EN_BIT         5
#define MPU6050_USERCTRL_I2C_IF_DIS_BIT         4
#define MPU6050_USERCTRL_DMP_RESET_BIT          3
#define MPU6050_USERCTRL_FIFO_RESET_BIT         2
#define MPU6050_USERCTRL_I2C_MST_RESET_BIT      1
#define MPU6050_USERCTRL_SIG_COND_RESET_BIT     0

#define MPU6050_PWR1_DEVICE_RESET_BIT   7
#define MPU6050_PWR1_SLEEP_BIT          6
#define MPU6050_PWR1_CYCLE_BIT          5
#define MPU6050_PWR1_TEMP_DIS_BIT       3
#define MPU6050_PWR1_CLKSEL_BIT         2
#define MPU6050_PWR1_CLKSEL_LENGTH      3

#define MPU6050_CLOCK_INTERNAL          0x00
#define MPU6050_CLOCK_PLL_XGYRO         0x01
#define MPU6050_CLOCK_PLL_YGYRO         0x02
#define MPU6050_CLOCK_PLL_ZGYRO         0x03
#define MPU6050_CLOCK_PLL_EXT32K        0x04
#define MPU6050_CLOCK_PLL_EXT19M        0x05
#define MPU6050_CLOCK_KEEP_RESET        0x07

#define MPU6050_PWR2_LP_WAKE_CTRL_BIT       7
#define MPU6050_PWR2_LP_WAKE_CTRL_LENGTH    2
#define MPU6050_PWR2_STBY_XA_BIT            5
#define MPU6050_PWR2_STBY_YA_BIT            4
#define MPU6050_PWR2_STBY_ZA_BIT            3
#define MPU6050_PWR2_STBY_XG_BIT            2
#define MPU6050_PWR2_STBY_YG_BIT            1
#define MPU6050_PWR2_STBY_ZG_BIT            0

#define MPU6050_WAKE_FREQ_1P25      0x0
#define MPU6050_WAKE_FREQ_2P5       0x1
#define MPU6050_WAKE_FREQ_5         0x2
#define MPU6050_WAKE_FREQ_10        0x3

#define MPU6050_BANKSEL_PRFTCH_EN_BIT       6
#define MPU6050_BANKSEL_CFG_USER_BANK_BIT   5
#define MPU6050_BANKSEL_MEM_SEL_BIT         4
#define MPU6050_BANKSEL_MEM_SEL_LENGTH      5

#define MPU6050_WHO_AM_I_BIT        6
#define MPU6050_WHO_AM_I_LENGTH     6

#define MPU6050_DMP_MEMORY_BANKS        8
#define MPU6050_DMP_MEMORY_BANK_SIZE    256
#define MPU6050_DMP_MEMORY_CHUNK_SIZE   16

#define MPU6050_FIFO_DEFAULT_TIMEOUT 11000
#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01

uint8_t mpuIntStatus;
uint16_t dmpPacketSize;
uint8_t SaveAddress;
bool dmpReady = false;
uint16_t packetSize = 42;
volatile bool mpuInterrupt = false;     // indicates whether MPU interrupt pin has gone high
uint16_t fifoCount;
uint8_t fifoBuffer[64]; 
float q[4];//w,x,y,z
int16_t aa[3];//x,y,z
float aaReal[3]; //x,y,z
float gravity[3];//x,y,z
float euler[3];         // [psi, theta, phi]    Euler angle container
float pry[3];           // [yaw, pitch, roll]   yaw/pitch/roll container and gravity vector

/*=================================================================================================*/

/*                                           工具函数                                              */

/*=================================================================================================*/



/*======================================I2C通讯=============================================*/



void dmpDataReady() {
    mpuInterrupt = true;
}

void write_mpu(byte reg,byte data){
  Wire.beginTransmission(mpu_addr);
  Wire.write(reg);
  Wire.write(data);
  Wire.endTransmission();
}

int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj){
  int8_t count = 0;
  uint32_t t1 = millis();
  for (int k = 0; k < length; k += min((int)length, 128)) {
    Wire.beginTransmission(devAddr);
    Wire.write(regAddr);
    Wire.endTransmission();
    Wire.requestFrom((uint8_t)devAddr, (uint8_t)min((int)length - k, 128));
    for (;Wire.available() && (timeout == 0 || millis() - t1 < timeout); count++) {
      data[count] = Wire.read();
    }
  }
  if (timeout > 0 && millis() - t1 >= timeout && count < length) count = -1; // timeout
  return count;
}

int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data, uint16_t timeout, void *wireObj) {
    return readBytes(devAddr, regAddr, 1, data, timeout, wireObj);
}

int8_t readBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t *data, uint16_t timeout, void *wireObj) {
    uint8_t b;
    uint8_t count = readByte(devAddr, regAddr, &b, timeout, wireObj);
    *data = b & (1 << bitNum);
    return count;
}

int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data, uint16_t timeout, void *wireObj) {
    // 01101001 read byte
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    //    010   masked
    //   -> 010 shifted
    uint8_t count, b;
    if ((count = readByte(devAddr, regAddr, &b, timeout, wireObj)) != 0) {
        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        b &= mask;
        b >>= (bitStart - length + 1);
        *data = b;
    }
    return count;
}

void writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data, void *wireObj) {
    //writeBytes(devAddr, regAddr, 1, &data, wireObj);
    Wire.beginTransmission(devAddr);
    Wire.write((uint8_t) regAddr); // send address
    for (uint8_t i = 0; i < length; i++) {
      Wire.write((uint8_t) data[i]);
    }
    Wire.endTransmission();
}

void writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data, void *wireObj) {
    return writeBytes(devAddr, regAddr, 1, &data, wireObj);
}

void writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data, void *wireObj){
  uint8_t b;
  uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
  data <<= (bitStart - length + 1); // shift data into correct position
  data &= mask; // zero all non-important bits in data
  b &= ~(mask); // zero all important bits in existing byte
  b |= data; // combine data with existing byte
  writeByte(devAddr, regAddr, b, wireObj);
}

void writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data, void *wireObj) {
    uint8_t b;
    readByte(devAddr, regAddr, &b, 1000, wireObj);
    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
    writeByte(devAddr, regAddr, b, wireObj);
}

void setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank) {
    bank &= 0x1F;
    if (userBank) bank |= 0x20;
    if (prefetchEnabled) bank |= 0x40;
    writeByte(mpu_addr, MPU6050_RA_BANK_SEL, bank, 0);
}

bool writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem) {
    setMemoryBank(bank,0,0);
    writeByte(mpu_addr, MPU6050_RA_MEM_START_ADDR, address, 0);
    uint8_t chunkSize;
    uint8_t *verifyBuffer=0;
    uint8_t *progBuffer=0;
    uint16_t i;
    uint8_t j;
    if (verify) verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    if (useProgMem) progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
    for (i = 0; i < dataSize;) {
        // determine correct chunk size according to bank position and data size
        chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;

        // make sure we don't go past the data size
        if (i + chunkSize > dataSize) chunkSize = dataSize - i;

        // make sure this chunk doesn't go past the bank boundary (256 bytes)
        if (chunkSize > 256 - address) chunkSize = 256 - address;
        
        if (useProgMem) {
            // write the chunk of data as specified
            for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j);
        } else {
            // write the chunk of data as specified
            progBuffer = (uint8_t *)data + i;
        }

        writeBytes(mpu_addr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer, 0);

        // verify data if needed
        if (verify && verifyBuffer) {
            setMemoryBank(bank,0,0);
            writeByte(mpu_addr, MPU6050_RA_MEM_START_ADDR, address, 0);
            readBytes(mpu_addr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer, 1000, 0);
            if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
                free(verifyBuffer);
                if (useProgMem) free(progBuffer);
                return false; // uh oh.
            }
        }

        // increase byte index by [chunkSize]
        i += chunkSize;

        // uint8_t automatically wraps to 0 at 256
        address += chunkSize;

        // if we aren't done, update bank (if necessary) and address
        if (i < dataSize) {
            if (address == 0) bank++;
            setMemoryBank(bank,0,0);
            writeByte(mpu_addr, MPU6050_RA_MEM_START_ADDR, address, 0);
        }
    }
    if (verify) free(verifyBuffer);
    if (useProgMem) free(progBuffer);
    return true;
}

bool writeWords(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint16_t* data, void *wireObj) {
    uint8_t status = 0;
    Wire.beginTransmission(devAddr);
    Wire.write(regAddr); // send address
    for (uint8_t i = 0; i < length; i++) { 
      Wire.write((uint8_t)(data[i] >> 8));    // send MSB
      Wire.write((uint8_t)data[i]);         // send LSB
    }
    status = Wire.endTransmission();
    return status == 0;
}

bool writeWord(uint8_t devAddr, uint8_t regAddr, uint16_t data, void *wireObj) {
    return writeWords(devAddr, regAddr, 1, &data, wireObj);
}

uint8_t getDeviceID() {
    readBits(mpu_addr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer, 1000, 0);
    return buffer[0];
}

uint8_t getIntStatus() {
    readByte(mpu_addr, MPU6050_RA_INT_STATUS, buffer, 1000, 0);
    return buffer[0];
}

uint16_t getFIFOCount() {
    readBytes(mpu_addr, MPU6050_RA_FIFO_COUNTH, 2, buffer, 1000, 0);
    return (((uint16_t)buffer[0]) << 8) | buffer[1];
}

void getFIFOBytes(uint8_t *data, uint8_t length) {
    if(length > 0){
        readBytes(mpu_addr, MPU6050_RA_FIFO_R_W, length, data, 1000, 0);
    } else {
    	*data = 0;
    }
}

/*===========================================计算===================================================*/



void getQuaternion(int16_t *data, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    data[0] = ((packet[0] << 8) | packet[1]);
    data[1] = ((packet[4] << 8) | packet[5]);
    data[2] = ((packet[8] << 8) | packet[9]);
    data[3] = ((packet[12] << 8) | packet[13]);
}

void dmpGetQuaternion(float *q, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    int16_t qI[4];
    getQuaternion(qI, packet);
    q[0] = (float)qI[0] / 16384.0f;
    q[1] = (float)qI[1] / 16384.0f;
    q[2] = (float)qI[2] / 16384.0f;
    q[3] = (float)qI[3] / 16384.0f;
}

void dmpGetGravity(float *v, float *q) {
  v[0] = 2 * (q[1]*q[3] - q[0]*q[2]);
  v[1] = 2 * (q[0]*q[1] + q[2]*q[3]);
  v[2] = q[0]*q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3];
}

void dmpGetPitchRollYaw(float *data, float *q, float *gravity) {
    // pitch: (nose up/down, about Y axis)
    data[0] = atan2(gravity[0] , sqrt(gravity[1]*gravity[1] + gravity[2]*gravity[2]));
    // roll: (tilt left/right, about X axis)
    data[1] = atan2(gravity[1] , gravity[2]);
    // yaw: (about Z axis)
    data[2] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);
    if (gravity[2] < 0) {
        if(data[1] > 0) {
            data[1] = PI - data[1]; 
        } else { 
            data[1] = -PI - data[1];
        }
    }
}

float map_(float x, float in_min, float in_max, float out_min, float out_max) {
    const float run = in_max - in_min;
    const float rise = out_max - out_min;
    const float delta = x - in_min;
    return (delta * rise) / run + out_min;
}

//注意：此函数当前参数为测试时测得，可能导致无法精确输出平动加速度，若更换接入的陀螺仪设备需自行校准！
int gravity_offset_table(float  val){
    if (val<0.05){
        return (int)map_(val,0,0.05,0,350);
    }else if(val<0.1){
        return (int)map_(val,0.05,0.1,350,760);
    }else if(val<0.15){
        return (int)map_(val,0.1,0.15,760,1080);
    }else if(val<0.2){
        return (int)map_(val,0.15,0.2,1080,1460);
    }else if(val<0.25){
        return (int)map_(val,0.2,0.25,1460,1850);
    }else if(val<0.3){
        return (int)map_(val,0.25,0.3,1850,2200);
    }else if(val<0.35){
        return (int)map_(val,0.3,0.35,2200,2550);
    }else if(val<0.4){
        return (int)map_(val,0.35,0.4,2550,2900);
    }else if(val<0.45){
        return (int)map_(val,0.4,0.45,2900,3270);
    }else if(val<0.5){
        return (int)map_(val,0.45,0.5,3270,3610);
    }else if(val<0.55){
        return (int)map_(val,0.5,0.55,3610,3990);
    }else if(val<0.6){
        return (int)map_(val,0.55,0.6,3990,4340);
    }else if(val<0.65){
        return (int)map_(val,0.6,0.65,4340,4710);
    }else if(val<0.7){
        return (int)map_(val,0.65,0.7,4710,5100);
    }else if(val<0.75){
        return (int)map_(val,0.7,0.75,5100,5480);
    }else if(val<0.8){
        return (int)map_(val,0.75,0.8,5480,5850);
    }else if(val<0.85){
        return (int)map_(val,0.8,0.85,5850,6210);
    }else if(val<0.9){
        return (int)map_(val,0.85,0.9,6210,6620);
    }else if(val<0.95){
        return (int)map_(val,0.9,0.95,6620,7100);
    }else{
        return (int)map_(val,0.95,1,7100,7610);
    }
}

int gravity_offset(float val){
    if(val>=0){
        return gravity_offset_table(val);
    }else{
        return -gravity_offset_table(-val);
    }
}

void dmpGetAccel(int16_t *v, const uint8_t* packet) {
    // TODO: accommodate different arrangements of sent data (ONLY default supported now)
    v[0] = (packet[28] << 8) | packet[29];
    v[1] = (packet[32] << 8) | packet[33];
    v[2] = (packet[36] << 8) | packet[37];
    v[2]+=1670;
}

void dmpGetLinearAccel(float *v, int16_t *vRaw, float *gravity) {
    v[0] = vRaw[0] - gravity_offset(gravity[0]);
    v[1] = vRaw[1] - gravity_offset(gravity[1]);
    v[2] = vRaw[2]-1675 - gravity[2]*6625;
    
}

void dmpGetEuler(float *pry, float *q) {
    pry[0] = -asin(2*q[1]*q[3] + 2*q[0]*q[2]);                              // theta
    pry[1] = atan2(2*q[2]*q[3] - 2*q[0]*q[1], 2*q[0]*q[0] + 2*q[3]*q[3] - 1);   // phi
    pry[2] = atan2(2*q[1]*q[2] - 2*q[0]*q[3], 2*q[0]*q[0] + 2*q[1]*q[1] - 1);   // psi
}


/*=====================================================显示===================================================*/

void show_gravity(){
    Serial.print("gravity:\t");
    Serial.print(gravity[0]);
    Serial.print("\t");
    Serial.print(gravity[1]);
    Serial.print("\t");
    Serial.println(gravity[2]);
}

void show_rotate(){
    Serial.print("PRY:\t");
    Serial.print(pry[0] * 180/M_PI);
    Serial.print("\t");
    Serial.print(pry[1] * 180/M_PI);
    Serial.print("\t");
    Serial.println(pry[2] * 180/M_PI);
}

void show_aa(){
    Serial.print("aa:\t");
    Serial.print(aa[0]);
    Serial.print("\t");
    Serial.print(aa[1]);
    Serial.print("\t");
    Serial.println(aa[2]);
}

void show_accel(){
    Serial.print("Accel:\t");
    Serial.print(aaReal[0]);
    Serial.print("\t");
    Serial.print(aaReal[1]);
    Serial.print("\t");
    Serial.println(aaReal[2]);
}

/*=================================================================================================*/

/*                                           初始化                                                */

/*=================================================================================================*/

void setup(){
    Serial.begin(115200);
    Wire.begin();
    write_mpu(0x6B,0x01);//初始化mpu6050姿态传感器
    write_mpu(0x1B,0x08);//设定陀螺仪量程+-500
    write_mpu(0x1C,0x00);//设定加速度计量程+-2g

    //initialize
    //writeBits(mpu_addr,0x6B,2,3,0x01,0);
    Serial.println("1");
    writeBits(mpu_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO, 0);
    //writeBits(mpu_addr,0x1B,4,2,0x00,0);
    Serial.println("2");
    writeBits(mpu_addr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250, 0);
    Serial.println("3");
    writeBits(mpu_addr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2, 0);
    Serial.println("4");
    writeBit(mpu_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, false,0);
    Serial.println("5");
    //initialize_end

    //dmpinitialize
    writeBit(mpu_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, true, 0);//reset
    writeBit(mpu_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, false, 0);
    setMemoryBank(0x10, true, true);
    writeByte(mpu_addr, MPU6050_RA_MEM_START_ADDR, 0x06, 0);
    readByte(mpu_addr, MPU6050_RA_MEM_R_W, buffer, 1000, 0);
    setMemoryBank(0, false, false);
    readBit(mpu_addr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, buffer, 1000, 0);
    writeByte(mpu_addr, MPU6050_RA_I2C_SLV0_ADDR, 0x7F, 0);
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, false,0);
    writeByte(mpu_addr, MPU6050_RA_I2C_SLV0_ADDR, 0x68, 0);
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, true, 0);
    writeBits(mpu_addr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO, 0);
    writeByte(mpu_addr, MPU6050_RA_INT_ENABLE, (1<<MPU6050_INTERRUPT_FIFO_OFLOW_BIT|1<<MPU6050_INTERRUPT_DMP_INT_BIT), 0);
    writeByte(mpu_addr, MPU6050_RA_SMPLRT_DIV, 4,0);
    writeBits(mpu_addr, MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L, 0);
    writeBits(mpu_addr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42, 0);
    writeBits(mpu_addr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000, 0);
    //writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 0, true);
    if (!writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 0, true)){
        Serial.println("no");
        delay(1000);
    }
    unsigned char dmpUpdate[] = {0x00, MPU6050_DMP_FIFO_RATE_DIVISOR};
    writeMemoryBlock(dmpUpdate, 0x02, 0x02, 0x16,0,0);
    writeByte(mpu_addr, MPU6050_RA_DMP_CFG_1, 0x03, 0);
    writeByte(mpu_addr, MPU6050_RA_DMP_CFG_2, 0x00, 0);
    writeBit(mpu_addr, MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, false, 0);
    writeByte(mpu_addr, MPU6050_RA_MOT_THR, 2, 0);
    writeByte(mpu_addr, MPU6050_RA_ZRMOT_THR, 156, 0);
    writeByte(mpu_addr, MPU6050_RA_MOT_DUR, 80, 0);
    writeByte(mpu_addr, MPU6050_RA_ZRMOT_DUR, 0, 0);
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, true, 0);
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, true, 0);
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, false, 0);
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true, 0);
    readByte(mpu_addr, MPU6050_RA_INT_STATUS, buffer, 1000, 0);

    writeWord(mpu_addr, MPU6050_RA_XG_OFFS_USRH, 220, 0);
    writeWord(mpu_addr, MPU6050_RA_YG_OFFS_USRH, 76, 0);
    writeWord(mpu_addr, MPU6050_RA_ZG_OFFS_USRH, 85, 0);
    SaveAddress = ((getDeviceID() < 0x38 )? MPU6050_RA_ZA_OFFS_H:0x7D);
    writeWord(mpu_addr, SaveAddress, 1788, 0);

    Serial.println(F("Enabling DMP..."));
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_EN_BIT, true,0);

    // enable Arduino interrupt detection
    Serial.println(F("Enabling interrupt detection (Arduino external interrupt 0)..."));
    attachInterrupt(0, dmpDataReady, RISING);
    mpuIntStatus=getIntStatus();

    // set our DMP Ready flag so the main loop() function knows it's okay to use it
    Serial.println(F("DMP ready! Waiting for first interrupt..."));
    dmpReady = true;

    // get expected DMP packet size for later comparison
    packetSize = 42;

}

/*=================================================================================================*/

/*                                            运行                                                 */

/*=================================================================================================*/

void loop(){
    mpuInterrupt = false;
    mpuIntStatus = getIntStatus();
    fifoCount = getFIFOCount();

if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
    // reset so we can continue cleanly
    writeBit(mpu_addr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, true, 0);
    Serial.println(F("FIFO overflow!"));

    // otherwise, check for DMP data ready interrupt (this should happen frequently)
} else if (mpuIntStatus & 0x02) {
    // wait for correct available data length, should be a VERY short wait
    while (fifoCount < packetSize) fifoCount = getFIFOCount();

    // read a packet from FIFO
    getFIFOBytes(fifoBuffer, packetSize);
    fifoCount -= packetSize;

    dmpGetQuaternion(q, fifoBuffer);
    dmpGetGravity(gravity, q);
    //dmpGetEuler(pry, q);//欧拉角计算（就是算法不同，输出结果和dmpGetGravity()等价）
    

    dmpGetPitchRollYaw(pry, q, gravity);//获取Pith，Roll，Yaw
    dmpGetAccel(aa, fifoBuffer);
    dmpGetLinearAccel(aaReal, aa, gravity);//获取实时加速度

    //show_gravity();//显示重力向量
    show_rotate();//显示旋转角度（Pitch，Roll，Yaw）
    //show_aa();//显示原始加速度数据（若不对平动加速度程序进行调试可以忽略）
    //show_accel();//显示平动加速度

  }
}

这段代码复制下来即可直接使用，接线方式为默认的I2C通讯接线：

​

3.3v->VCC

GND->GND

GPIO22->SCL

GPIO21->SDA

3.效果

运行效果如下：

​

colour64
关注
MPU6050 Arduino：实时获取角度数据并进行姿态估计
HwfvTest的博客
10-10 1339
连接时，将MPU6050的VCC引脚连接到Arduino的5V引脚，GND引脚连接到Arduino的GND引脚，SCL引脚连接到Arduino的A5引脚（或者SCL引脚），SDA引脚连接到Arduino的A4引脚（或者SDA引脚）。使用Arduino与MPU6050传感器进行通信，我们可以通过读取传感器的原始数据并进行计算，得到姿态角度。将代码上传至Arduino开发板后，打开串口监视器，你将看到MPU6050传感器实时输出的滚转角、俯仰角和偏航角数据。在代码中，首先包含了所需的库文件。
[Arduino ESP32] mpu6050使用笔记(含dmp）
qq_60438746的博客
01-25 5163
基于vscode+PlatformIO,使用Arduino开发ESP32介绍了MPU6050库的使用(含DMP读取)和出现的问题。
ESP32——MPU6050
sfghdfgh的博客
07-15 2159
使用ESP32简单编写MPU6050程序并使用
Arduino中使用库文件读取陀螺仪MPU6050欧拉角
最新发布
panjinliang066333的博客
09-24 800
重新启动Arduino软件，即可找到MPU6050库文件提供的案例。打开例子，使用例子需要做一点修改。或者下载下面改好的例子。解压后将文件夹复制到如下文件路径，重启软件即可。在库中搜索mpu6050，然后安装它。如果找不到可以点击本链接，下载。找到如图所示的版本进行安装。找到方法1中下载的压缩文件。
Arduino提高篇（十七）MPU6050姿态解算
2301_81461100的博客
01-02 2515
对于大多数MPU6050的应用来说，获取到的原始数据并没有多大用处，我们需要对原始数据进行姿态融合解算，最终得到姿态数据，也就是三个欧拉角：航向角(yaw)、横滚角(roll)和俯仰角(pitch)。MPU6050内部自带数字运动处理器(DMP)硬件加速引擎，配合运动驱动库直接输出四元数，进而很方便的计算出欧拉角，大大降低了主控MCU的负担。本篇使用MPU6050的驱动库来获取姿态数据。1. MPU6050驱动库安装。
【ESP32 Arduino平衡小车制作】（二) mpu6050 dmp欧拉角解算 PlatformIO
weixin_45829708的博客
05-11 5936
mpu6050 dmp6 欧拉角解算 MPU6050一款常用的集成加速度计的陀螺仪。今天就来介绍ESP32 Arduino如何使用mpu6050做dmp ，四元数姿态解算，从而获得欧拉角和xyz角。 最后的效果：可以看到很好的实验效果，eular和ypr角。 此外我使用的是esp32 PICO4芯片，PlatformIO开发环境 mpu6050接PIN32,PIN 33引脚 一、库安装： 用过PlatformIO的朋友们肯定知道，PlatformIO具有丰富的库文件，使用来那是十分的方便。在库中搜素mpu
ESP32+MPU-6050.rar
05-31
使用ESP32控制MPU6050， 并且使用DMP输出姿态角
Arduino ESP32通过MPU6050控制两个自由度舵机
stars_n的博客
12-22 2560
一开始想利用mpu6050做一个云台或者陀螺仪体感控制，利用mpu6050的方向数据来反馈给单片机并对舵机进行控制，于是就有了本篇教程 当使用 ESP32 来控制两个自由度舵机时，通常需要一个传感器来获取姿态信息，而MPU6050 是一个常用的加速度计和陀螺仪传感器组合，适用于测量姿态。以下只是一个简单的教程，演示如何通过 MPU6050 控制两个舵机，并使用 Arduino IDE 进行编程。
ESP32 IDF平台 MPU6050 DMP 驱动及对应的测试上位机源码
03-15
文件夹包含了: - 0 官方库文件 MD5.1.3 与 MD6.12 两个版本的官方库文件。 - 1 ESP32 IDF 平台MPU DMP驱动文件 移植好的ESP32 IDF 平台MPU DMP驱动文件。 - 2 测试工程 已经测试后的测试工程。 - 3 上位机源码与exe 及上位机的源码和打包发布了的应用程序 mpu_display.exe。
ESP32学习：MPU6050
sssxlxwbwz的博客
12-26 9858
一、概念介绍 1. MPU6050基础介绍 MPU6050 是 InvenSense 公司推出的全球首款整合性 6 轴运动处理组件，相较于多组件方案，免除了组合陀螺仪与加速器时之轴间差的问题，减少了安装空间。 MPU6050 内部整合了 3 轴陀螺仪和 3 轴加速度传感器，并且含有一个第二 IIC 接口，可用于连接外部磁力传感器，并利用自带的数字运动处理器（DMP: Digital Motion Processor）硬件加速引擎，通过主 IIC 接口，向应用端输出...
mpu6050/9250——DMP姿态解算.rar
05-30
标题中的“mpu6050/9250——DMP姿态解算.rar”指的是一个关于MPU6050和MPU9250传感器的数字运动处理（Digital Motion Processing, DMP）姿态解算项目。MPU6050是一款六轴惯性测量单元（IMU），包含三轴加速度计和三...
arduino mpu6050 dmp库文件
12-08
arduino mpu6050 dmp库文件，用于mpu6050 内部dmp调用
ESP32-IDF平台 mpu6050 测试工程
03-12
ESP32-IDF平台 mpu6050 测试工程。 用寄存器的方式控制与读取数据。
ESP32 MPU6050 驱动以及姿态算法解析
08-27
1. ESP32-S3 IIC驱动MPU6050 2. 集成官方姿态算法DMP解析库 3. VScode编译
Arduino提高篇17—MPU6050姿态解算
01-06
对于大多数MPU6050的应用来说，获取到的原始数据并没有多大用处，我们需要对原始数据进行姿态融合解算，最终得到姿态数据，也就是三个欧拉角：航向角(yaw)、横滚角(roll)和俯仰角(pitch)。 MPU6050内部自带数字运动处理器(DMP)硬件加速引擎，配合运动驱动库直接输出四元数，进而很方便的计算出欧拉角，大大降低了主控MCU的负担。本篇使用MPU6050的驱动库来获取姿态数据。 1. MPU6050驱动库安装 MPU6050的驱动库有很多，我们可以在IDE中单击「项目」—「加载库」—「管理库」，在搜索栏输入”6050″，可以看到不同的驱动库。 本篇我们使用的由国外大牛Jeff R
Arduino uno + mpu6050 陀螺仪 运用卡尔曼滤波姿态解算源代码(总共4套程序)
01-19
在本项目中，我们关注的是如何使用Arduino Uno与MPU6050陀螺仪进行姿态解算，并结合卡尔曼滤波技术优化数据输出。这是一个关于嵌入式系统、传感器技术和信号处理的综合实践。 首先，MPU6050是一款集成的六轴惯性...
Arduino IDE开发ESP32&MPU6050，DMP库解算姿态最快速的方法
weixin_54606734的博客
03-08 1715
arduino ide开发esp32，利用dmp库解算mpu6050原始数据
ESP32设备驱动：MPU6050加速度计陀螺仪和温度传感器驱动
GwtbPerl的博客
09-22 775
将模块的VCC引脚连接到ESP32的3.3V电源引脚，GND引脚连接到ESP32的地引脚，SCL引脚连接到ESP32的I2C时钟引脚（通常是GPIO22），SDA引脚连接到ESP32的I2C数据引脚（通常是GPIO21）。本文将介绍如何在ESP32上驱动MPU6050加速度计陀螺仪和温度传感器，为你的物联网项目提供准确的姿态和环境数据。通过以上步骤，你成功地驱动了MPU6050加速度计陀螺仪和温度传感器，并将数据输出到ESP32的串口监视器。你将看到每秒钟打印出MPU6050的加速度、陀螺仪和温度数据。
esp32连接mpu6050
blog.boringhex.top
07-21 1373
MPU-6050是InvenSense公司生产的一款六轴运动处理器，集成了3轴加速度计和3轴陀螺仪。它将两个传感器集成到一个极小的封装里，使得对六轴运动的检测变得更加简单和方便。这款传感器广泛应用于运动控制和测量领域，如无人机、智能手机、运动手环等。这一步主要设置工程名称、工程目录、目标板芯片、串口，还有需要添加的软件组件目录。微信搜一搜 “funio”，发现更多精彩内容。，与mpu6050寄存器手册描述一致。内置数字运动处理器（DMP）公众号 | FunIO。
esp32 arduino mpu6050姿态解算
07-01
### 回答1： ESP32 Arduino MPU6050是一种常用的传感器组合，用于姿态解算。它结合了ESP32开发板、Arduino编程环境和MPU6050模块。 MPU6050是一个基于MEMS技术的六轴陀螺仪加速度计模块。它可以测量和输出设备的加速度和角速度数据。而ESP32是一款功能强大的微控制器，具有Wi-Fi和蓝牙功能。结合这两个设备，我们可以实现姿态解算。 姿态解算是通过传感器获得的角速度和加速度数据来推算设备的方向和旋转角度。通过读取MPU6050模块的数据，我们可以了解设备的姿态。 在Arduino编程环境中，我们可以使用相应的库函数来读取MPU6050的数据，并通过姿态解算算法来计算设备的姿态。常用的姿态解算算法包括卡尔曼滤波器、四元数解算和欧拉角解算。 使用ESP32 Arduino库和MPU6050库，我们可以很容易地实现MPU6050的姿态解算。首先，我们需要连接MPU6050模块到ESP32开发板上的对应引脚。然后，使用库函数来初始化和读取MPU6050的数据。最后，根据选择的姿态解算算法，计算设备的姿态信息。 姿态解算可以应用于许多领域，如飞行控制、机器人导航和虚拟现实。通过使用ESP32 Arduino和MPU6050，我们可以方便地实现姿态解算功能，并开发各种应用。 ### 回答2： ESP32 Arduino MPU6050姿态解算是指使用ESP32开发板和MPU6050陀螺仪加速度计模块来实现姿态解算功能。MPU6050是一种集成了三轴陀螺仪和三轴加速度计的传感器，可以用来检测物体的倾斜角度和姿态变化。 首先，我们需要通过Arduino库文件连接ESP32和MPU6050模块。然后，通过配置相关寄存器设置传感器的采样频率和测量范围。接下来，我们可以通过读取传感器的原始数值来获取加速度和角速度的数据。 为了获取准确的倾斜角度和姿态信息，我们需要对原始数据进行处理。其中一个常用的方法是使用卡尔曼滤波算法，它可以通过将测量数据与模型预测结果结合起来，来减小噪音的影响并提高测量精度。 在进行姿态解算之前，我们需要进行传感器校准。这可以通过将传感器放置在平稳的表面上，并记录静态状态下的加速度和角速度数据，然后计算出零偏校准参数。这样可以减小误差并提高精度。 一旦传感器校准完成，我们可以通过使用姿态解算算法（如：马德格弗斯滤波器或四元数法）来获取精确的姿态数据。这些算法可以将加速度和角速度数据转换为旋转矩阵或四元数，并进一步计算出倾斜角度、滚动角度和偏航角等相关信息。 最后，我们可以将姿态数据应用于实际应用中，例如控制机器人、导航系统或者平衡控制等。通过ESP32 Arduino MPU6050姿态解算，可以实现精确的姿态监测和控制。
评论 10
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付：10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明：

1.余额是钱包充值的虚拟货币，按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载，可以购买VIP、付费专栏及课程。

余额充值