MPU-6050 DMP Arduino code for quaternion, Euler, YPR, more

Hey everyone,

The MPU6050 code library I've been working on (part of the I2Cdevlib project), and more specifically the Arduino DMP6 example sketch, has just been given a nice functionality/convenience boost.

This code compiles to about 18kB total flash memory usage and runs flawlessly on an Arduino Uno. By uncommenting different #define macros, you can get DMP-powered output in the following formats:

#OUTPUT_READABLE_QUATERNION
Actual quaternion components in a [w, x, y, z] format

#OUTPUT_READABLE_EULER
Euler angles (in degrees) calculated from the quaternions coming from the FIFO. Note that Euler angles suffer from gimbal lock.

#OUTPUT_READABLE_YAWPITCHROLL
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.

#OUTPUT_READABLE_FRAMEACCEL
Linear acceleration components with gravity removed and adjusted for the initial "zero rotation" frame of reference. Could be quite handy in some cases. This mode also reads directly from the raw sensor registers in addition to using the FIFO for DMP output.

#OUTPUT_TEAPOT
Output that matches the format used for the InvenSense teapot demo.

Note that all of these output methods rely on the DMP to produce quaternions only. Conversion to Euler angles, Y/P/R, gravity vector detection, and compensated acceleration are done in the code below, not directly in the DMP.

Arduino sketch code is here (same link as above).

Have fun!