mpu6050 dmp test

Dependencies:   mbed

Fork of MPU6050-DMP_6DOF by quadcopter cufe

Embed: (wiki syntax)

« Back to documentation index

Show/hide line numbers MPU6050_6Axis_MotionApps20.h Source File

MPU6050_6Axis_MotionApps20.h

00001 // I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation
00002 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
00003 // 5/20/2013 by Jeff Rowberg <jeff@rowberg.net>
00004 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
00005 //
00006 // Changelog:
00007 //     ... - ongoing debug release
00008 
00009 /* ============================================
00010 I2Cdev device library code is placed under the MIT license
00011 Copyright (c) 2012 Jeff Rowberg
00012 
00013 Permission is hereby granted, free of charge, to any person obtaining a copy
00014 of this software and associated documentation files (the "Software"), to deal
00015 in the Software without restriction, including without limitation the rights
00016 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00017 copies of the Software, and to permit persons to whom the Software is
00018 furnished to do so, subject to the following conditions:
00019 
00020 The above copyright notice and this permission notice shall be included in
00021 all copies or substantial portions of the Software.
00022 
00023 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00024 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00025 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00026 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00027 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00028 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00029 THE SOFTWARE.
00030 ===============================================
00031 */
00032 
00033 #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_
00034 #define _MPU6050_6AXIS_MOTIONAPPS20_H_
00035 
00036 #include <iostream>
00037 
00038 #include "I2Cdev.h"
00039 #include "helper_3dmath.h"
00040 
00041 // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board
00042 #define MPU6050_INCLUDE_DMP_MOTIONAPPS20
00043 
00044 #include "MPU6050.h"
00045 
00046 // Tom Carpenter's conditional PROGMEM code
00047 // http://forum.arduino.cc/index.php?topic=129407.0
00048 #ifndef __arm__
00049     #include <avr/pgmspace.h>
00050 #else
00051     // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
00052     #ifndef __PGMSPACE_H_
00053         #define __PGMSPACE_H_ 1
00054         #include <inttypes.h>
00055 
00056         #define PROGMEM
00057         #define PGM_P  const char *
00058         #define PSTR(str) (str)
00059         #define F(x) x
00060 
00061         typedef void prog_void;
00062         typedef char prog_char;
00063         typedef unsigned char prog_uchar;
00064         typedef int8_t prog_int8_t;
00065         typedef uint8_t prog_uint8_t;
00066         typedef int16_t prog_int16_t;
00067         typedef uint16_t prog_uint16_t;
00068         typedef int32_t prog_int32_t;
00069         typedef uint32_t prog_uint32_t;
00070         
00071         #define strcpy_P(dest, src) strcpy((dest), (src))
00072         #define strcat_P(dest, src) strcat((dest), (src))
00073         #define strcmp_P(a, b) strcmp((a), (b))
00074         
00075         #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
00076         #define pgm_read_word(addr) (*(const unsigned short *)(addr))
00077         #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
00078         #define pgm_read_float(addr) (*(const float *)(addr))
00079         
00080         #define pgm_read_byte_near(addr) pgm_read_byte(addr)
00081         #define pgm_read_word_near(addr) pgm_read_word(addr)
00082         #define pgm_read_dword_near(addr) pgm_read_dword(addr)
00083         #define pgm_read_float_near(addr) pgm_read_float(addr)
00084         #define pgm_read_byte_far(addr) pgm_read_byte(addr)
00085         #define pgm_read_word_far(addr) pgm_read_word(addr)
00086         #define pgm_read_dword_far(addr) pgm_read_dword(addr)
00087         #define pgm_read_float_far(addr) pgm_read_float(addr)
00088     #endif
00089 #endif
00090 
00091 /* Source is from the InvenSense MotionApps v2 demo code. Original source is
00092  * unavailable, unless you happen to be amazing as decompiling binary by
00093  * hand (in which case, please contact me, and I'm totally serious).
00094  *
00095  * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the
00096  * DMP reverse-engineering he did to help make this bit of wizardry
00097  * possible.
00098  */
00099 
00100 // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
00101 // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
00102 // after moving string constants to flash memory storage using the F()
00103 // compiler macro (Arduino IDE 1.0+ required).
00104 
00105 //#define DEBUG
00106 #ifdef DEBUG
00107     #define DEBUG_PRINT(x) pc.printf(x)   //Serial.print(x)
00108     #define DEBUG_PRINTF(x, y) pc.printf(x,y)   //Serial.print(x, y)
00109 
00110     #define F(x) x
00111 #else
00112     #define DEBUG_PRINT(x)
00113     #define DEBUG_PRINTF(x, y)
00114 
00115 #endif
00116 
00117 #define MPU6050_DMP_CODE_SIZE       1929    // dmpMemory[]
00118 #define MPU6050_DMP_CONFIG_SIZE     192     // dmpConfig[]
00119 #define MPU6050_DMP_UPDATES_SIZE    47      // dmpUpdates[]
00120 
00121 /* ================================================================================================ *
00122  | Default MotionApps v2.0 42-byte FIFO packet structure:                                           |
00123  |                                                                                                  |
00124  | [QUAT W][      ][QUAT X][      ][QUAT Y][      ][QUAT Z][      ][GYRO X][      ][GYRO Y][      ] |
00125  |   0   1   2   3   4   5   6   7   8   9  10  11  12  13  14  15  16  17  18  19  20  21  22  23  |
00126  |                                                                                                  |
00127  | [GYRO Z][      ][ACC X ][      ][ACC Y ][      ][ACC Z ][      ][      ]                         |
00128  |  24  25  26  27  28  29  30  31  32  33  34  35  36  37  38  39  40  41                          |
00129  * ================================================================================================ */
00130 
00131 // this block of memory gets written to the MPU on start-up, and it seems
00132 // to be volatile memory, so it has to be done each time (it only takes ~1
00133 // second though)
00134 const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
00135     // bank 0, 256 bytes
00136     0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
00137     0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
00138     0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00139     0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
00140     0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
00141     0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00142     0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
00143     0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
00144     0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
00145     0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
00146     0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
00147     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
00148     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00149     0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
00150     0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
00151     0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
00152 
00153     // bank 1, 256 bytes
00154     0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00155     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
00156     0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
00157     0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
00158     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
00159     0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
00160     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
00161     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00162     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00163     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00164     0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
00165     0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
00166     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
00167     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00168     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00169     0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
00170 
00171     // bank 2, 256 bytes
00172     0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00173     0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
00174     0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
00175     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00176     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00177     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00178     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00179     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00180     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00181     0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00182     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00183     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
00184     0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00185     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00186     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00187     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00188 
00189     // bank 3, 256 bytes
00190     0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F,
00191     0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2,
00192     0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF,
00193     0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C,
00194     0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1,
00195     0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01,
00196     0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80,
00197     0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C,
00198     0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80,
00199     0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E,
00200     0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9,
00201     0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24,
00202     0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0,
00203     0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86,
00204     0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
00205     0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
00206 
00207     // bank 4, 256 bytes
00208     0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
00209     0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
00210     0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
00211     0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
00212     0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
00213     0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
00214     0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
00215     0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
00216     0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
00217     0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
00218     0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
00219     0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
00220     0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
00221     0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
00222     0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
00223     0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
00224 
00225     // bank 5, 256 bytes
00226     0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
00227     0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
00228     0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
00229     0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
00230     0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
00231     0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
00232     0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
00233     0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
00234     0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A,
00235     0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60,
00236     0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97,
00237     0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04,
00238     0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78,
00239     0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79,
00240     0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68,
00241     0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68,
00242 
00243     // bank 6, 256 bytes
00244     0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04,
00245     0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66,
00246     0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31,
00247     0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60,
00248     0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76,
00249     0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56,
00250     0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD,
00251     0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91,
00252     0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8,
00253     0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE,
00254     0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9,
00255     0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD,
00256     0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E,
00257     0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8,
00258     0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89,
00259     0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79,
00260 
00261     // bank 7, 138 bytes (remainder)
00262     0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8,
00263     0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA,
00264     0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB,
00265     0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3,
00266     0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3,
00267     0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
00268     0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3,
00269     0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC,
00270     0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
00271 };
00272 
00273 // thanks to Noah Zerkin for piecing this stuff together!
00274 const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
00275 //  BANK    OFFSET  LENGTH  [DATA]
00276     0x03,   0x7B,   0x03,   0x4C, 0xCD, 0x6C,         // FCFG_1 inv_set_gyro_calibration
00277     0x03,   0xAB,   0x03,   0x36, 0x56, 0x76,         // FCFG_3 inv_set_gyro_calibration
00278     0x00,   0x68,   0x04,   0x02, 0xCB, 0x47, 0xA2,   // D_0_104 inv_set_gyro_calibration
00279     0x02,   0x18,   0x04,   0x00, 0x05, 0x8B, 0xC1,   // D_0_24 inv_set_gyro_calibration
00280     0x01,   0x0C,   0x04,   0x00, 0x00, 0x00, 0x00,   // D_1_152 inv_set_accel_calibration
00281     0x03,   0x7F,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration
00282     0x03,   0x89,   0x03,   0x26, 0x46, 0x66,         // FCFG_7 inv_set_accel_calibration
00283     0x00,   0x6C,   0x02,   0x20, 0x00,               // D_0_108 inv_set_accel_calibration
00284     0x02,   0x40,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_00 inv_set_compass_calibration
00285     0x02,   0x44,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_01
00286     0x02,   0x48,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_02
00287     0x02,   0x4C,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_10
00288     0x02,   0x50,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_11
00289     0x02,   0x54,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_12
00290     0x02,   0x58,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_20
00291     0x02,   0x5C,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_21
00292     0x02,   0xBC,   0x04,   0x00, 0x00, 0x00, 0x00,   // CPASS_MTX_22
00293     0x01,   0xEC,   0x04,   0x00, 0x00, 0x40, 0x00,   // D_1_236 inv_apply_endian_accel
00294     0x03,   0x7F,   0x06,   0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
00295     0x04,   0x02,   0x03,   0x0D, 0x35, 0x5D,         // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
00296     0x04,   0x09,   0x04,   0x87, 0x2D, 0x35, 0x3D,   // FCFG_5 inv_set_bias_update
00297     0x00,   0xA3,   0x01,   0x00,                     // D_0_163 inv_set_dead_zone
00298                  // SPECIAL 0x01 = enable interrupts
00299     0x00,   0x00,   0x00,   0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION
00300     0x07,   0x86,   0x01,   0xFE,                     // CFG_6 inv_set_fifo_interupt
00301     0x07,   0x41,   0x05,   0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
00302     0x07,   0x7E,   0x01,   0x30,                     // CFG_16 inv_set_footer
00303     0x07,   0x46,   0x01,   0x9A,                     // CFG_GYRO_SOURCE inv_send_gyro
00304     0x07,   0x47,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_9 inv_send_gyro -> inv_construct3_fifo
00305     0x07,   0x6C,   0x04,   0xF1, 0x28, 0x30, 0x38,   // CFG_12 inv_send_accel -> inv_construct3_fifo
00306     0x02,   0x16,   0x02,   0x00, 0x01                // D_0_22 inv_set_fifo_rate
00307 
00308     // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz,
00309     // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data.
00310     // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value))
00311 
00312     // It is important to make sure the host processor can keep up with reading and processing
00313     // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea.
00314 };
00315 
00316 const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
00317     0x01,   0xB2,   0x02,   0xFF, 0xFF,
00318     0x01,   0x90,   0x04,   0x09, 0x23, 0xA1, 0x35,
00319     0x01,   0x6A,   0x02,   0x06, 0x00,
00320     0x01,   0x60,   0x08,   0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00321     0x00,   0x60,   0x04,   0x40, 0x00, 0x00, 0x00,
00322     0x01,   0x62,   0x02,   0x00, 0x00,
00323     0x00,   0x60,   0x04,   0x00, 0x40, 0x00, 0x00
00324 };
00325 
00326 uint8_t MPU6050::dmpInitialize() {
00327     // reset device
00328     Serial pc(USBTX, USBRX); // tx, rx  
00329     pc.baud(115200);
00330     wait_ms(50);
00331     
00332     pc.printf("\n\nSet USB Baudrate to 115200...\n");
00333     
00334     pc.printf("\n\nResetting MPU6050...\n");
00335     reset();
00336 
00337     wait_ms(30);
00338 
00339     // enable sleep mode and wake cycle
00340     /*Serial.println(F("Enabling sleep mode..."));
00341     setSleepEnabled(true);
00342     Serial.println(F("Enabling wake cycle..."));
00343     setWakeCycleEnabled(true);*/
00344 
00345     // disable sleep mode
00346     DEBUG_PRINT("Disabling sleep mode...\n");
00347     setSleepEnabled(false);
00348 
00349     // get MPU hardware revision
00350     DEBUG_PRINT("Selecting user bank 16...\n");
00351     setMemoryBank(0x10, true, true);
00352     DEBUG_PRINT("Selecting memory byte 6...\n");
00353     setMemoryStartAddress(0x06);
00354     DEBUG_PRINT("Checking hardware revision...\n");
00355     uint8_t hwRevision = readMemoryByte();
00356     DEBUG_PRINT("Revision @ user[16][6] = ");
00357     DEBUG_PRINTF("%x\n",hwRevision);
00358     DEBUG_PRINT("Resetting memory bank selection to 0...\n");
00359     setMemoryBank(0, false, false);
00360 
00361     // check OTP bank valid
00362     DEBUG_PRINT("Reading OTP bank valid flag...\n");
00363     uint8_t otpValid = getOTPBankValid();
00364     
00365     DEBUG_PRINT("OTP bank is ");
00366     if(otpValid)  DEBUG_PRINT("valid!\n");
00367     else  DEBUG_PRINT("invalid!\n");
00368 
00369     // get X/Y/Z gyro offsets
00370     /*
00371     DEBUG_PRINT("\nReading gyro offset TC values...\n");
00372     int8_t xgOffsetTC = mpu.getXGyroOffsetTC();
00373     int8_t ygOffsetTC = getYGyroOffsetTC();
00374     int8_t zgOffsetTC = getZGyroOffsetTC();
00375     DEBUG_PRINTF("X gyro offset = %u\n",xgOffset);
00376     DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset);
00377     DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset);
00378     */
00379     // setup weird slave stuff (?)
00380     DEBUG_PRINT("Setting slave 0 address to 0x7F...\n");
00381     setSlaveAddress(0, 0x7F);
00382     
00383     DEBUG_PRINT("Disabling I2C Master mode...");
00384     setI2CMasterModeEnabled(false);
00385     DEBUG_PRINT("Setting slave 0 address to 0x68 (self)...");
00386     setSlaveAddress(0, 0x68);
00387     DEBUG_PRINT("Resetting I2C Master control...\n");
00388     resetI2CMaster();
00389 
00390     wait_ms(20);
00391 
00392     // load DMP code into memory banks
00393     DEBUG_PRINT("Writing DMP code to MPU memory banks (");
00394     DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE);
00395     DEBUG_PRINT(" bytes)\n");
00396     if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
00397          DEBUG_PRINT("Success! DMP code written and verified.\n");
00398 
00399         // write DMP configuration
00400         DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
00401         DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE);
00402         DEBUG_PRINT(" bytes in config def)\n");
00403         if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
00404             DEBUG_PRINT("Success! DMP configuration written and verified.\n");
00405 
00406             DEBUG_PRINT("Setting clock source to Z Gyro...\n");
00407             setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
00408 
00409             DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
00410             setIntEnabled(0x12);
00411 
00412             DEBUG_PRINT("Setting sample rate to 200Hz...");
00413             setRate(4); // 1khz / (1 + 4) = 200 Hz
00414 
00415             DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n");
00416             setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
00417 
00418             DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n");
00419             setDLPFMode(MPU6050_DLPF_BW_42);
00420 
00421             DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
00422             setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00423 
00424             DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n");
00425             setDMPConfig1(0x03);
00426             setDMPConfig2(0x00);
00427 
00428             DEBUG_PRINT("Clearing OTP Bank flag...");
00429             setOTPBankValid(false);
00430 
00431             DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n");
00432             //setXGyroOffsetTC(xgOffsetTC);
00433             //setYGyroOffsetTC(ygOffsetTC);
00434             //setZGyroOffsetTC(zgOffsetTC);
00435 
00436             //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
00437             //setXGyroOffset(0);
00438             //setYGyroOffset(0);
00439             //setZGyroOffset(0);
00440 
00441             DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n");
00442             uint8_t dmpUpdate[16], j;
00443             uint16_t pos = 0;
00444             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00445             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00446 
00447             DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n");
00448             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00449             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00450 
00451             DEBUG_PRINT("Resetting FIFO...\n");
00452             resetFIFO();
00453 
00454             DEBUG_PRINT("Reading FIFO count...\n");
00455             uint16_t fifoCount = getFIFOCount();
00456             uint8_t fifoBuffer[128];
00457 
00458             DEBUG_PRINT("Current FIFO count=");
00459             DEBUG_PRINTF("%u\n",fifoCount);
00460             getFIFOBytes(fifoBuffer, fifoCount);
00461 
00462             DEBUG_PRINT("Setting motion detection threshold to 2...\n");
00463             setMotionDetectionThreshold(2);
00464 
00465             DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n");
00466             setZeroMotionDetectionThreshold(156);
00467 
00468             DEBUG_PRINT("Setting motion detection duration to 80...");
00469             setMotionDetectionDuration(80);
00470 
00471             DEBUG_PRINT("Setting zero-motion detection duration to 0...");
00472             setZeroMotionDetectionDuration(0);
00473 
00474             DEBUG_PRINT("Resetting FIFO...\n");
00475             resetFIFO();
00476 
00477             DEBUG_PRINT("Enabling FIFO...\n");
00478             setFIFOEnabled(true);
00479 
00480             DEBUG_PRINT("Enabling DMP...\n");
00481             setDMPEnabled(true);
00482 
00483             DEBUG_PRINT("Resetting DMP...\n");
00484             resetDMP();
00485 
00486             DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n");
00487             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00488             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00489 
00490             DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n");
00491             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00492             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00493 
00494             DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n");
00495             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00496             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00497 
00498             DEBUG_PRINT("Waiting for FIFO count > 2...\n");
00499             while ((fifoCount = getFIFOCount()) < 3);
00500 
00501             DEBUG_PRINT("Current FIFO count=");
00502             DEBUG_PRINTF("%u\n",fifoCount);
00503             DEBUG_PRINT("Reading FIFO data...\n");
00504             getFIFOBytes(fifoBuffer, fifoCount);
00505 
00506             DEBUG_PRINT("Reading interrupt status...\n");
00507             uint8_t mpuIntStatus = getIntStatus();
00508 
00509             DEBUG_PRINT("Current interrupt status=");
00510             DEBUG_PRINTF("%x\n",mpuIntStatus);
00511 
00512             DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n");
00513             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00514             readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00515 
00516             DEBUG_PRINT("Waiting for FIFO count > 2...\n");
00517             while ((fifoCount = getFIFOCount()) < 3);
00518 
00519             DEBUG_PRINT("Current FIFO count=");
00520             DEBUG_PRINTF("%u\n",fifoCount);
00521 
00522             DEBUG_PRINT("Reading FIFO data...\n");
00523             getFIFOBytes(fifoBuffer, fifoCount);
00524 
00525             DEBUG_PRINT("Reading interrupt status...\n");
00526             mpuIntStatus = getIntStatus();
00527 
00528             DEBUG_PRINT("Current interrupt status=");
00529             DEBUG_PRINTF("%x\n",mpuIntStatus);
00530 
00531             DEBUG_PRINT("Writing final memory update 7/7 (function unknown)...");
00532             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00533             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00534 
00535             DEBUG_PRINT("DMP is good to go! Finally.\n");
00536 
00537             DEBUG_PRINT("Disabling DMP (you turn it on later)...\n");
00538             setDMPEnabled(false);
00539 
00540             DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\n");
00541             dmpPacketSize = 42;
00542             /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
00543                 return 3; // TODO: proper error code for no memory
00544             }*/
00545 
00546             DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n");
00547             resetFIFO();
00548             getIntStatus();
00549         } else {
00550             DEBUG_PRINT("ERROR! DMP configuration verification failed.\n");
00551             return 2; // configuration block loading failed
00552         }
00553     } else {
00554         DEBUG_PRINT("ERROR! DMP code verification failed.");
00555         return 1; // main binary block loading failed
00556     }
00557     return 0; // success
00558 }
00559 
00560 bool MPU6050::dmpPacketAvailable() {
00561     return getFIFOCount() >= dmpGetFIFOPacketSize();
00562 }
00563 
00564 // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
00565 // uint8_t MPU6050::dmpGetFIFORate();
00566 // uint8_t MPU6050::dmpGetSampleStepSizeMS();
00567 // uint8_t MPU6050::dmpGetSampleFrequency();
00568 // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
00569 
00570 //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
00571 //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
00572 //uint8_t MPU6050::dmpRunFIFORateProcesses();
00573 
00574 // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
00575 // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
00576 // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00577 // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00578 // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
00579 // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
00580 // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
00581 // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
00582 // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
00583 // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
00584 // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00585 // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
00586 
00587 uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
00588     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00589     if (packet == 0) packet = dmpPacketBuffer;
00590     data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]);
00591     data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]);
00592     data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]);
00593     return 0;
00594 }
00595 uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
00596     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00597     if (packet == 0) packet = dmpPacketBuffer;
00598     data[0] = (packet[28] << 8) + packet[29];
00599     data[1] = (packet[32] << 8) + packet[33];
00600     data[2] = (packet[36] << 8) + packet[37];
00601     return 0;
00602 }
00603 uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
00604     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00605     if (packet == 0) packet = dmpPacketBuffer;
00606     v -> x = (packet[28] << 8) + packet[29];
00607     v -> y = (packet[32] << 8) + packet[33];
00608     v -> z = (packet[36] << 8) + packet[37];
00609     return 0;
00610 }
00611 uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
00612     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00613     if (packet == 0) packet = dmpPacketBuffer;
00614     data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]);
00615     data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]);
00616     data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]);
00617     data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]);
00618     return 0;
00619 }
00620 uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
00621     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00622     if (packet == 0) packet = dmpPacketBuffer;
00623     data[0] = ((packet[0] << 8) + packet[1]);
00624     data[1] = ((packet[4] << 8) + packet[5]);
00625     data[2] = ((packet[8] << 8) + packet[9]);
00626     data[3] = ((packet[12] << 8) + packet[13]);
00627     return 0;
00628 }
00629 uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
00630     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00631     int16_t qI[4];
00632     uint8_t status = dmpGetQuaternion(qI, packet);
00633     if (status == 0) {
00634         q -> w = (float)qI[0] / 16384.0f;
00635         q -> x = (float)qI[1] / 16384.0f;
00636         q -> y = (float)qI[2] / 16384.0f;
00637         q -> z = (float)qI[3] / 16384.0f;
00638         return 0;
00639     }
00640     return status; // int16 return value, indicates error if this line is reached
00641 }
00642 // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
00643 // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
00644 uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
00645     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00646     if (packet == 0) packet = dmpPacketBuffer;
00647     data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]);
00648     data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]);
00649     data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);
00650     return 0;
00651 }
00652 uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
00653     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00654     if (packet == 0) packet = dmpPacketBuffer;
00655     data[0] = (packet[16] << 8) + packet[17];
00656     data[1] = (packet[20] << 8) + packet[21];
00657     data[2] = (packet[24] << 8) + packet[25];
00658     return 0;
00659 }
00660 // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
00661 // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
00662 uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
00663     // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
00664     v -> x = vRaw -> x - gravity -> x*8192;
00665     v -> y = vRaw -> y - gravity -> y*8192;
00666     v -> z = vRaw -> z - gravity -> z*8192;
00667     return 0;
00668 }
00669 // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
00670 uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
00671     // rotate measured 3D acceleration vector into original state
00672     // frame of reference based on orientation quaternion
00673     memcpy(v, vReal, sizeof(VectorInt16));
00674     v -> rotate(q);
00675     return 0;
00676 }
00677 // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
00678 // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
00679 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
00680 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
00681 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
00682 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
00683     v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
00684     v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
00685     v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
00686     return 0;
00687 }
00688 // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
00689 // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
00690 // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
00691 // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
00692 
00693 uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
00694     data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);   // psi
00695     data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y);                              // theta
00696     data[2] = atan2(2*q -> y*q -> z - 2*q -> w*q -> x, 2*q -> w*q -> w + 2*q -> z*q -> z - 1);   // phi
00697     return 0;
00698 }
00699 
00700 uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
00701     // yaw: (about Z axis)
00702     data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
00703     // pitch: (nose up/down, about Y axis)
00704     data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
00705     // roll: (tilt left/right, about X axis)
00706     data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
00707     return 0;
00708 }
00709 
00710 // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
00711 // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
00712 
00713 uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
00714     /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
00715         if (dmpData[k] < 0x10) Serial.print("0");
00716         Serial.print(dmpData[k], HEX);
00717         Serial.print(" ");
00718     }
00719     Serial.print("\n");*/
00720     //Serial.println((uint16_t)dmpPacketBuffer);
00721     return 0;
00722 }
00723 uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
00724     uint8_t status;
00725     uint8_t buf[dmpPacketSize];
00726     for (uint8_t i = 0; i < numPackets; i++) {
00727         // read packet from FIFO
00728         getFIFOBytes(buf, dmpPacketSize);
00729 
00730         // process packet
00731         if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
00732         
00733         // increment external process count variable, if supplied
00734         if (processed != 0) *processed++;
00735     }
00736     return 0;
00737 }
00738 
00739 // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
00740 
00741 // uint8_t MPU6050::dmpInitFIFOParam();
00742 // uint8_t MPU6050::dmpCloseFIFO();
00743 // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
00744 // uint8_t MPU6050::dmpDecodeQuantizedAccel();
00745 // uint32_t MPU6050::dmpGetGyroSumOfSquare();
00746 // uint32_t MPU6050::dmpGetAccelSumOfSquare();
00747 // void MPU6050::dmpOverrideQuaternion(long *q);
00748 uint16_t MPU6050::dmpGetFIFOPacketSize() {
00749     return dmpPacketSize;
00750 }
00751 
00752 #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */