本文主要解析MPU6050 DMP解算示例，对程序进行了汉语注释，方便阅读，喜欢的可以看一看。准备做一个自巡航小车，欢迎评论交了，等待续集。。。
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2019-07-08 - Added Auto Calibration and offset generator
// - and altered FIFO retrieval sequence to avoid using blocking code
// 2016-04-18 - Eliminated a potential infinite loop
// 2013-05-08 - added seamless Fastwire support
// - added note about gyro calibration
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
// 2012-06-20 - improved FIFO overflow handling and simplified read process
// 2012-06-19 - completely rearranged DMP initialization code and simplification
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
// - add 3D math helper file to DMP6 example sketch
// - add Euler output and Yaw/Pitch/Roll output formats
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
// 2012-05-30 - basic DMP initialization working
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
/* =========================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
depends on the MPU-6050's INT pin being connected to the Arduino's
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
digital I/O pin 2.
* ========================================================================= */
/* =========================================================================
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
when using Serial.write(buf, len). The Teapot output uses this method.
The solution requires a modification to the Arduino USBAPI.h file, which
is fortunately simple, but annoying. This will be fixed in the next IDE
release. For more info, see these links:
http://arduino.cc/forum/index.php/topic,109987.0.html
http://code.google.com/p/arduino/issues/detail?id=958
* ========================================================================= */
//=========以下六项可选择输出，非注释行即为输出项================================================
// 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 //以度为单位）根据来自FIFO的四元数计算。以度为单位显示欧拉角度
// 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 //如果要查看加速度，请取消注释“OUTPUT_READABLE_WORLDACCEL”
// 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 //以InvenSense茶壶演示格式显示四元数值：
//===========================================================================================
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful 如果DMP初始化成功，则设置为true
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 保存来自MPU的实际中断状态字节
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 每次设备操作后返回状态（0=成功，！0=错误）
uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 预期的DMP数据包大小（默认值为42字节）
uint16_t fifoCount; // count of all bytes currently in FIFO 当前FIFO中所有字节的计数
uint8_t fifoBuffer[64]; // FIFO storage buffer FIFO存储缓冲器
// 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 InvenSense茶壶演示的数据包结构
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE === 中断检测程序
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP === 初始设置
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically) 加入I2C总线（I2Cdev库不会自动执行此操作）
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties //400kHz I2C时钟。如果有编译困难，请对此行进行注释
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.
// initialize device 初始化设备
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection 验证连接
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready 等待准备完成
Serial.println(F("\nSend any character to begin DMP programming and demo: ")); //\发送任意字符开始DMP编程和演示：
while (Serial.available() && Serial.read()); // empty buffer 清空缓冲区
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP 加载和配置DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity 在这里提供您自己的陀螺仪偏移，按最小灵敏度进行缩放
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 1688我的测试芯片的出厂默认值
// make sure it worked (returns 0 if so) 确保它有效（如果有效，则返回0）
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050 校准时间：生成偏移并校准我们的MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready 打开DMP，现在它已经准备好了
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection 启用Arduino中断检测
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it 设置DMP Ready标志，使main loop（）函数知道可以使用它
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison 获取预期的DMP数据包大小以便稍后进行比较
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===主程序循环
// ================================================================
void loop() {
// if programming failed, don't try to do anything 如果编程失败，不要尝试做任何事情
if (!dmpReady) return;
// read a packet from FIFO 从FIFO读取数据包
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 获取最新数据包
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z 以简单矩阵形式显示四元数值：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); //mpu.dmpGetEuler 是一个计算欧拉角（Euler Angles）的函数。
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.dmpGetGravity 是一个计算重力向量的函数
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //mpu.dmpGetYawPitchRoll 是一个计算雅可比矩阵（Jacobian Matrix）的函数
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.dmpGetAccel 是一个计算加速度的函数
mpu.dmpGetGravity(&gravity, &q); //mpu.dmpGetGravity 是一个计算重力向量的函数
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); //mpu.dmpGetLinearAccel 是一个计算线性加速度的函数
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);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
// display quaternion values in InvenSense Teapot demo format: 以InvenSense茶壶演示格式显示四元数值：
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose //packetCount，故意在0xFF处循环
#endif
// blink LED to indicate activity LED闪烁以指示活动
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
//
// Changelog:
// 2019-07-08 - Added Auto Calibration and offset generator
// - and altered FIFO retrieval sequence to avoid using blocking code
// 2016-04-18 - Eliminated a potential infinite loop
// 2013-05-08 - added seamless Fastwire support
// - added note about gyro calibration
// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
// 2012-06-20 - improved FIFO overflow handling and simplified read process
// 2012-06-19 - completely rearranged DMP initialization code and simplification
// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
// - add 3D math helper file to DMP6 example sketch
// - add Euler output and Yaw/Pitch/Roll output formats
// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
// 2012-05-30 - basic DMP initialization working
/* ============================================
I2Cdev device library code is placed under the MIT license
Copyright (c) 2012 Jeff Rowberg
Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:
The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
===============================================
*/
// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
// for both classes must be in the include path of your project
#include "I2Cdev.h"
#include "MPU6050_6Axis_MotionApps20.h"
//#include "MPU6050.h" // not necessary if using MotionApps include file
// Arduino Wire library is required if I2Cdev I2CDEV_ARDUINO_WIRE implementation
// is used in I2Cdev.h
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
#include "Wire.h"
#endif
// class default I2C address is 0x68
// specific I2C addresses may be passed as a parameter here
// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
// AD0 high = 0x69
MPU6050 mpu;
//MPU6050 mpu(0x69); // <-- use for AD0 high
/* =========================================================================
NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
depends on the MPU-6050's INT pin being connected to the Arduino's
external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
digital I/O pin 2.
* ========================================================================= */
/* =========================================================================
NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
when using Serial.write(buf, len). The Teapot output uses this method.
The solution requires a modification to the Arduino USBAPI.h file, which
is fortunately simple, but annoying. This will be fixed in the next IDE
release. For more info, see these links:
http://arduino.cc/forum/index.php/topic,109987.0.html
http://code.google.com/p/arduino/issues/detail?id=958
* ========================================================================= */
// 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 //以度为单位）根据来自FIFO的四元数计算。以度为单位显示欧拉角度
// 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 //如果要查看加速度，请取消注释“OUTPUT_READABLE_WORLDACCEL”
// 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 //以InvenSense茶壶演示格式显示四元数值：
#define INTERRUPT_PIN 2 // use pin 2 on Arduino Uno & most boards
#define LED_PIN 13 // (Arduino is 13, Teensy is 11, Teensy++ is 6)
bool blinkState = false;
// MPU control/status vars
bool dmpReady = false; // set true if DMP init was successful 如果DMP初始化成功，则设置为true
uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU 保存来自MPU的实际中断状态字节
uint8_t devStatus; // return status after each device operation (0 = success, !0 = error) 每次设备操作后返回状态（0=成功，！0=错误）
uint16_t packetSize; // expected DMP packet size (default is 42 bytes) 预期的DMP数据包大小（默认值为42字节）
uint16_t fifoCount; // count of all bytes currently in FIFO 当前FIFO中所有字节的计数
uint8_t fifoBuffer[64]; // FIFO storage buffer FIFO存储缓冲器
// 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 InvenSense茶壶演示的数据包结构
uint8_t teapotPacket[14] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n' };
// ================================================================
// === INTERRUPT DETECTION ROUTINE === 中断检测程序
// ================================================================
volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
void dmpDataReady() {
mpuInterrupt = true;
}
// ================================================================
// === INITIAL SETUP === 初始设置
// ================================================================
void setup() {
// join I2C bus (I2Cdev library doesn't do this automatically) 加入I2C总线（I2Cdev库不会自动执行此操作）
#if I2CDEV_IMPLEMENTATION == I2CDEV_ARDUINO_WIRE
Wire.begin();
Wire.setClock(400000); // 400kHz I2C clock. Comment this line if having compilation difficulties //400kHz I2C时钟。如果有编译困难，请对此行进行注释
#elif I2CDEV_IMPLEMENTATION == I2CDEV_BUILTIN_FASTWIRE
Fastwire::setup(400, true);
#endif
// initialize serial communication
// (115200 chosen because it is required for Teapot Demo output, but it's
// really up to you depending on your project)
Serial.begin(115200);
while (!Serial); // wait for Leonardo enumeration, others continue immediately
// NOTE: 8MHz or slower host processors, like the Teensy @ 3.3V or Arduino
// Pro Mini running at 3.3V, cannot handle this baud rate reliably due to
// the baud timing being too misaligned with processor ticks. You must use
// 38400 or slower in these cases, or use some kind of external separate
// crystal solution for the UART timer.
// initialize device 初始化设备
Serial.println(F("Initializing I2C devices..."));
mpu.initialize();
pinMode(INTERRUPT_PIN, INPUT);
// verify connection 验证连接
Serial.println(F("Testing device connections..."));
Serial.println(mpu.testConnection() ? F("MPU6050 connection successful") : F("MPU6050 connection failed"));
// wait for ready 等待准备完成
Serial.println(F("\nSend any character to begin DMP programming and demo: ")); //\发送任意字符开始DMP编程和演示：
while (Serial.available() && Serial.read()); // empty buffer 清空缓冲区
while (!Serial.available()); // wait for data
while (Serial.available() && Serial.read()); // empty buffer again
// load and configure the DMP 加载和配置DMP
Serial.println(F("Initializing DMP..."));
devStatus = mpu.dmpInitialize();
// supply your own gyro offsets here, scaled for min sensitivity 在这里提供您自己的陀螺仪偏移，按最小灵敏度进行缩放
mpu.setXGyroOffset(220);
mpu.setYGyroOffset(76);
mpu.setZGyroOffset(-85);
mpu.setZAccelOffset(1788); // 1688 factory default for my test chip 1688我的测试芯片的出厂默认值
// make sure it worked (returns 0 if so) 确保它有效（如果有效，则返回0）
if (devStatus == 0) {
// Calibration Time: generate offsets and calibrate our MPU6050 校准时间：生成偏移并校准我们的MPU6050
mpu.CalibrateAccel(6);
mpu.CalibrateGyro(6);
mpu.PrintActiveOffsets();
// turn on the DMP, now that it's ready 打开DMP，现在它已经准备好了
Serial.println(F("Enabling DMP..."));
mpu.setDMPEnabled(true);
// enable Arduino interrupt detection 启用Arduino中断检测
Serial.print(F("Enabling interrupt detection (Arduino external interrupt "));
Serial.print(digitalPinToInterrupt(INTERRUPT_PIN));
Serial.println(F(")..."));
attachInterrupt(digitalPinToInterrupt(INTERRUPT_PIN), dmpDataReady, RISING);
mpuIntStatus = mpu.getIntStatus();
// set our DMP Ready flag so the main loop() function knows it's okay to use it 设置DMP Ready标志，使main loop（）函数知道可以使用它
Serial.println(F("DMP ready! Waiting for first interrupt..."));
dmpReady = true;
// get expected DMP packet size for later comparison 获取预期的DMP数据包大小以便稍后进行比较
packetSize = mpu.dmpGetFIFOPacketSize();
} else {
// ERROR!
// 1 = initial memory load failed
// 2 = DMP configuration updates failed
// (if it's going to break, usually the code will be 1)
Serial.print(F("DMP Initialization failed (code "));
Serial.print(devStatus);
Serial.println(F(")"));
}
// configure LED for output
pinMode(LED_PIN, OUTPUT);
}
// ================================================================
// === MAIN PROGRAM LOOP ===主程序循环
// ================================================================
void loop() {
// if programming failed, don't try to do anything 如果编程失败，不要尝试做任何事情
if (!dmpReady) return;
// read a packet from FIFO 从FIFO读取数据包
if (mpu.dmpGetCurrentFIFOPacket(fifoBuffer)) { // Get the Latest packet 获取最新数据包
#ifdef OUTPUT_READABLE_QUATERNION
// display quaternion values in easy matrix form: w x y z 以简单矩阵形式显示四元数值：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); //mpu.dmpGetEuler 是一个计算欧拉角（Euler Angles）的函数。
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.dmpGetGravity 是一个计算重力向量的函数
mpu.dmpGetYawPitchRoll(ypr, &q, &gravity); //mpu.dmpGetYawPitchRoll 是一个计算雅可比矩阵（Jacobian Matrix）的函数
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.dmpGetAccel 是一个计算加速度的函数
mpu.dmpGetGravity(&gravity, &q); //mpu.dmpGetGravity 是一个计算重力向量的函数
mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity); //mpu.dmpGetLinearAccel 是一个计算线性加速度的函数
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);
Serial.print("\t");
Serial.print(aaWorld.y);
Serial.print("\t");
Serial.println(aaWorld.z);
#endif
#ifdef OUTPUT_TEAPOT
// display quaternion values in InvenSense Teapot demo format: 以InvenSense茶壶演示格式显示四元数值：
teapotPacket[2] = fifoBuffer[0];
teapotPacket[3] = fifoBuffer[1];
teapotPacket[4] = fifoBuffer[4];
teapotPacket[5] = fifoBuffer[5];
teapotPacket[6] = fifoBuffer[8];
teapotPacket[7] = fifoBuffer[9];
teapotPacket[8] = fifoBuffer[12];
teapotPacket[9] = fifoBuffer[13];
Serial.write(teapotPacket, 14);
teapotPacket[11]++; // packetCount, loops at 0xFF on purpose //packetCount，故意在0xFF处循环
#endif
// blink LED to indicate activity LED闪烁以指示活动
blinkState = !blinkState;
digitalWrite(LED_PIN, blinkState);
}
}
2万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
