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_9Axis_MotionApps41.h Source File

MPU6050_9Axis_MotionApps41.h

00001 // I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation
00002 // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00)
00003 // 6/18/2012 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_9AXIS_MOTIONAPPS41_H_
00034 #define _MPU6050_9AXIS_MOTIONAPPS41_H_
00035 
00036 #include <iostream>
00037 
00038 #include "I2Cdev.h"
00039 #include "helper_3dmath.h"
00040 
00041 // MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board
00042 #define MPU6050_INCLUDE_DMP_MOTIONAPPS41
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 #define strcpy_P(dest, src) strcpy((dest), (src))
00071 #define strcat_P(dest, src) strcat((dest), (src))
00072 #define strcmp_P(a, b) strcmp((a), (b))
00073 #define pgm_read_byte(addr) (*(const unsigned char *)(addr))
00074 #define pgm_read_word(addr) (*(const unsigned short *)(addr))
00075 #define pgm_read_dword(addr) (*(const unsigned long *)(addr))
00076 #define pgm_read_float(addr) (*(const float *)(addr))
00077 #define pgm_read_byte_near(addr) pgm_read_byte(addr)
00078 #define pgm_read_word_near(addr) pgm_read_word(addr)
00079 #define pgm_read_dword_near(addr) pgm_read_dword(addr)
00080 #define pgm_read_float_near(addr) pgm_read_float(addr)
00081 #define pgm_read_byte_far(addr) pgm_read_byte(addr)
00082 #define pgm_read_word_far(addr) pgm_read_word(addr)
00083 #define pgm_read_dword_far(addr) pgm_read_dword(addr)
00084 #define pgm_read_float_far(addr) pgm_read_float(addr)
00085 #endif
00086 #endif
00087 
00088 #define min(a,b) (((a)<(b))?(a):(b))
00089 
00090 // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size.
00091 // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno)
00092 // after moving string constants to flash memory storage using the F()
00093 // compiler macro (Arduino IDE 1.0+ required).
00094 
00095 #define DEBUG
00096 #ifdef DEBUG
00097     #define DEBUG_PRINT(x) pc.printf(x)   //Serial.print(x)
00098     #define DEBUG_PRINTF(x, y) pc.printf(x,y)   //Serial.print(x, y)
00099 //#define DEBUG_PRINTLN(x) Serial.println(x)
00100 //#define DEBUG_PRINTLNF(x, y) Serial.println(x, y)
00101 #else
00102 #define DEBUG_PRINT(x)
00103 #define DEBUG_PRINTF(x, y)
00104 //#define DEBUG_PRINTLN(x)
00105 //#define DEBUG_PRINTLNF(x, y)
00106 #endif
00107 
00108 #define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[]
00109 #define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[]
00110 #define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[]
00111 
00112 /* ================================================================================================ *
00113 | Default MotionApps v4.1 48-byte FIFO packet structure: |
00114 | |
00115 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] |
00116 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 |
00117 | |
00118 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] |
00119 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 |
00120 * ================================================================================================ */
00121 
00122 // this block of memory gets written to the MPU on start-up, and it seems
00123 // to be volatile memory, so it has to be done each time (it only takes ~1
00124 // second though)
00125 const prog_uchar dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
00126     // bank 0, 256 bytes
00127     0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00,
00128     0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01,
00129     0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00130     0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00,
00131     0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01,
00132     0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00133     0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00,
00134     0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00,
00135     0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82,
00136     0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00,
00137     0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00,
00138     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0,
00139     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00140     0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC,
00141     0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4,
00142     0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10,
00143 
00144     // bank 1, 256 bytes
00145     0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00146     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00,
00147     0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8,
00148     0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7,
00149     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C,
00150     0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00,
00151     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14,
00152     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00153     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00154     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00155     0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C,
00156     0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00,
00157     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30,
00158     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00159     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00160     0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0,
00161     
00162     // bank 2, 256 bytes
00163     0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00164     0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00,
00165     0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00,
00166     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00167     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00168     0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00169     0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00170     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00171     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00172     0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00173     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00174     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00,
00175     0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00176     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00177     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2,
00178     0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00179     
00180     // bank 3, 256 bytes
00181     0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4,
00182     0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA,
00183     0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80,
00184     0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0,
00185     0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1,
00186     0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3,
00187     0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88,
00188     0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF,
00189     0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89,
00190     0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9,
00191     0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A,
00192     0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11,
00193     0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55,
00194     0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54,
00195     0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D,
00196     0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86,
00197     
00198     // bank 4, 256 bytes
00199     0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1,
00200     0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86,
00201     0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA,
00202     0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C,
00203     0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8,
00204     0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3,
00205     0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84,
00206     0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5,
00207     0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3,
00208     0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1,
00209     0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5,
00210     0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D,
00211     0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
00212     0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D,
00213     0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9,
00214     0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A,
00215     
00216     // bank 5, 256 bytes
00217     0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8,
00218     0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87,
00219     0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8,
00220     0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68,
00221     0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D,
00222     0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94,
00223     0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA,
00224     0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56,
00225     0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9,
00226     0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA,
00227     0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86,
00228     0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40,
00229     0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9,
00230     0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30,
00231     0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0,
00232     0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24,
00233 
00234     // bank 6, 256 bytes
00235     0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40,
00236     0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71,
00237     0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0,
00238     0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02,
00239     0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38,
00240     0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19,
00241     0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86,
00242     0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A,
00243     0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E,
00244     0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55,
00245     0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D,
00246     0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51,
00247     0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19,
00248     0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9,
00249     0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76,
00250     0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC,
00251     
00252     // bank 7, 170 bytes (remainder)
00253     0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24,
00254     0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9,
00255     0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D,
00256     0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D,
00257     0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34,
00258     0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9,
00259     0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3,
00260     0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3,
00261     0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3,
00262     0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3,
00263     0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF
00264 };
00265 
00266 const prog_uchar dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
00267 // BANK OFFSET LENGTH [DATA]
00268     0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ?
00269     0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration
00270     0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration
00271     0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration
00272     0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration
00273     0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration
00274     0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration
00275     0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration
00276 
00277     0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration
00278     0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01
00279     0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02
00280     0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10
00281     0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11
00282     0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12
00283     0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20
00284     0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21
00285     0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22
00286 
00287     0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel
00288     0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors
00289     0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion
00290     0x00, 0xA3, 0x01, 0x00, // ?
00291     0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update
00292     0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion
00293     0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer
00294     0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro
00295     0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo
00296     0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ?
00297     0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ?
00298     0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ?
00299                  // SPECIAL 0x01 = enable interrupts
00300     0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION
00301     0x07, 0xA7, 0x01, 0xFE, // ?
00302     0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ?
00303     0x07, 0x67, 0x01, 0x9A, // ?
00304     0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo
00305     0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo
00306     0x02, 0x16, 0x02, 0x00, 0x03 // 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 prog_uchar dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
00317     0x01, 0xB2, 0x02, 0xFF, 0xF5,
00318     0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0,
00319     0x00, 0xA3, 0x01, 0x00,
00320     0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D,
00321     0x01, 0x6A, 0x02, 0x06, 0x00,
00322     0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00323     0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
00324     0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
00325     0x01, 0x08, 0x02, 0x01, 0x20,
00326     0x01, 0x0A, 0x02, 0x00, 0x4E,
00327     0x01, 0x02, 0x02, 0xFE, 0xB3,
00328     0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ
00329     0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00,
00330     0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C,
00331     0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00,
00332     0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00,
00333     0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00,
00334     0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00,
00335     0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00
00336 };
00337 
00338 uint8_t MPU6050::dmpInitialize() {
00339     Serial pc(USBTX, USBRX); // tx, rx  
00340     pc.baud(115200);
00341     wait_ms(50);
00342     
00343     pc.printf("\n\nSet USB Baudrate to 115200...\n");
00344     
00345     // reset device
00346     DEBUG_PRINT("\n\nResetting MPU6050...");
00347     reset();
00348     wait_ms(30);
00349     
00350     // disable sleep mode
00351     DEBUG_PRINT("Disabling sleep mode...");
00352     setSleepEnabled(false);
00353 
00354     // get MPU product ID
00355 //    DEBUG_PRINT("Getting product ID...");
00356 //    uint8_t productID = 0; 
00357 //    getProductID();
00358 //    DEBUG_PRINT("Product ID = ");
00359 //    DEBUG_PRINTF("%u", productID);
00360 
00361     // get MPU hardware revision
00362     DEBUG_PRINT("Selecting user bank 16...\n");
00363     setMemoryBank(0x10, true, true);
00364     DEBUG_PRINT("Selecting memory byte 6...\n");
00365     setMemoryStartAddress(0x06);
00366     DEBUG_PRINT("Checking hardware revision...\n");
00367     uint8_t hwRevision = readMemoryByte();
00368     DEBUG_PRINT("Revision @ user[16][6] = ");
00369     DEBUG_PRINTF("%x\n",hwRevision);
00370     DEBUG_PRINT("Resetting memory bank selection to 0...");
00371     setMemoryBank(0, false, false);
00372 
00373     // check OTP bank valid
00374     DEBUG_PRINT("Reading OTP bank valid flag...\n");
00375     uint8_t otpValid = getOTPBankValid();
00376     DEBUG_PRINT("OTP bank is ");
00377    
00378     if(otpValid)DEBUG_PRINT("valid!\n");
00379     else DEBUG_PRINT("invalid!\n");
00380 
00381     // get X/Y/Z gyro offsets
00382     DEBUG_PRINT("Reading gyro offset values...\n");
00383     int8_t xgOffset = getXGyroOffset();
00384     int8_t ygOffset = getYGyroOffset();
00385     int8_t zgOffset = getZGyroOffset();
00386     DEBUG_PRINT("X gyro offset = ");
00387     DEBUG_PRINTF("%u\n",xgOffset);
00388     DEBUG_PRINT("Y gyro offset = ");
00389     DEBUG_PRINTF("%u\n",ygOffset);
00390     DEBUG_PRINT("Z gyro offset = ");
00391     DEBUG_PRINTF("%u\n",zgOffset);
00392     
00393     i2Cdev.readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ?
00394     
00395     DEBUG_PRINT("Enabling interrupt latch, clear on any read, AUX bypass enabled\n");
00396     i2Cdev.writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32);
00397 
00398     // enable MPU AUX I2C bypass mode
00399     //DEBUG_PRINT("Enabling AUX I2C bypass mode..."));
00400     //setI2CBypassEnabled(true);
00401 
00402     DEBUG_PRINT("Setting magnetometer mode to power-down...\n");
00403     //mag -> setMode(0);
00404     i2Cdev.writeByte(0x0E, 0x0A, 0x00);
00405 
00406     DEBUG_PRINT("Setting magnetometer mode to fuse access...\n");
00407     //mag -> setMode(0x0F);
00408     i2Cdev.writeByte(0x0E, 0x0A, 0x0F);
00409 
00410     DEBUG_PRINT("Reading mag magnetometer factory calibration...\n");
00411     
00412     int8_t asax, asay, asaz;
00413     
00414     //mag -> getAdjustment(&asax, &asay, &asaz);
00415     
00416     i2Cdev.readBytes(0x0E, 0x10, 3, buffer);
00417     asax = (int8_t)buffer[0];
00418     asay = (int8_t)buffer[1];
00419     asaz = (int8_t)buffer[2];
00420     DEBUG_PRINT("Adjustment X/Y/Z = ");
00421     DEBUG_PRINTF("%u",asax);
00422     DEBUG_PRINT(" / ");
00423     DEBUG_PRINTF("%u",asay);
00424     DEBUG_PRINT(" / ");
00425     DEBUG_PRINTF("%u\n",asaz);
00426 
00427     DEBUG_PRINT("Setting magnetometer mode to power-down...\n");
00428     //mag -> setMode(0);
00429     i2Cdev.writeByte(0x0E, 0x0A, 0x00);
00430 
00431     // load DMP code into memory banks
00432     DEBUG_PRINT("Writing DMP code to MPU memory banks (");
00433     DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE);
00434     DEBUG_PRINT(" bytes)\n");
00435     if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
00436         DEBUG_PRINT("Success! DMP code written and verified.\n");
00437 
00438         DEBUG_PRINT("Configuring DMP and related settings...\n");
00439 
00440         // write DMP configuration
00441         DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
00442         DEBUG_PRINTF("%u ",MPU6050_DMP_CONFIG_SIZE);
00443         DEBUG_PRINT(" bytes in config def)\n");
00444         if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
00445             DEBUG_PRINT("Success! DMP configuration written and verified.\n");
00446 
00447             DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
00448             setIntEnabled(0x12);
00449 
00450             DEBUG_PRINT("Setting sample rate to 200Hz...\n");
00451             setRate(4); // 1khz / (1 + 4) = 200 Hz
00452 
00453             DEBUG_PRINT("Setting clock source to Z Gyro...\n");
00454             setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
00455 
00456             DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n");
00457             setDLPFMode(MPU6050_DLPF_BW_42);
00458 
00459             DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n");
00460             setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
00461 
00462             DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
00463             setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
00464 
00465             DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n");
00466             setDMPConfig1(0x03);
00467             setDMPConfig2(0x00);
00468 
00469             DEBUG_PRINT("Clearing OTP Bank flag...\n");
00470             setOTPBankValid(false);
00471 
00472             DEBUG_PRINT("Setting X/Y/Z gyro offsets to previous values...\n");
00473             setXGyroOffset(xgOffset);
00474             setYGyroOffset(ygOffset);
00475             setZGyroOffset(zgOffset);
00476 
00477             DEBUG_PRINT("Setting X/Y/Z gyro user offsets to zero...\n");
00478             setXGyroOffsetUser(0);
00479             setYGyroOffsetUser(0);
00480             setZGyroOffsetUser(0);
00481 
00482             DEBUG_PRINT("Writing final memory update 1/19 (function unknown)...\n");
00483             uint8_t dmpUpdate[16],j;
00484             uint16_t pos = 0;
00485             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00486             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00487 
00488             DEBUG_PRINT("Writing final memory update 2/19 (function unknown)...\n");
00489             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00490             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00491 
00492             DEBUG_PRINT("Resetting FIFO...\n");
00493             resetFIFO();
00494 
00495             DEBUG_PRINT("Reading FIFO count...\n");
00496             uint8_t fifoCount = getFIFOCount();
00497 
00498             DEBUG_PRINT("Current FIFO count=");
00499             DEBUG_PRINTF("%u\n",fifoCount);
00500             uint8_t fifoBuffer[128];
00501             getFIFOBytes(fifoBuffer, fifoCount);
00502 
00503             DEBUG_PRINT("Writing final memory update 3/19 (function unknown)...\n");
00504             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00505             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00506 
00507             DEBUG_PRINT("Writing final memory update 4/19 (function unknown)...\n");
00508             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00509             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00510 
00511             DEBUG_PRINT("Disabling all standby flags...\n");
00512             i2Cdev.writeByte(0x68, MPU6050_RA_PWR_MGMT_2, 0x00);
00513 
00514             DEBUG_PRINT("Setting accelerometer sensitivity to +/- 2g...\n");
00515             i2Cdev.writeByte(0x68, MPU6050_RA_ACCEL_CONFIG, 0x00);
00516 
00517             DEBUG_PRINT("Setting motion detection threshold to 2...\n");
00518             setMotionDetectionThreshold(2);
00519 
00520             DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n");
00521             setZeroMotionDetectionThreshold(156);
00522 
00523             DEBUG_PRINT("Setting motion detection duration to 80...\n");
00524             setMotionDetectionDuration(80);
00525 
00526             DEBUG_PRINT("Setting zero-motion detection duration to 0...\n");
00527             setZeroMotionDetectionDuration(0);
00528 
00529             DEBUG_PRINT("Setting AK8975 to single measurement mode...\n");
00530             //mag -> setMode(1);
00531             i2Cdev.writeByte(0x0E, 0x0A, 0x01);
00532 
00533             // setup AK8975 (0x0E) as Slave 0 in read mode
00534             DEBUG_PRINT("Setting up AK8975 read slave 0...\n");
00535             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E);
00536             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01);
00537             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA);
00538 
00539             // setup AK8975 (0x0E) as Slave 2 in write mode
00540             DEBUG_PRINT("Setting up AK8975 write slave 2...\n");
00541             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E);
00542             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A);
00543             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81);
00544             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01);
00545 
00546             // setup I2C timing/delay control
00547             DEBUG_PRINT("Setting up slave access delay...\n");
00548             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
00549             i2Cdev.writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
00550 
00551             // enable interrupts
00552             DEBUG_PRINT("Enabling default interrupt behavior/no bypass...\n");
00553             i2Cdev.writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00);
00554 
00555             // enable I2C master mode and reset DMP/FIFO
00556             DEBUG_PRINT("Enabling I2C master mode...\n");
00557             i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
00558             DEBUG_PRINT("Resetting FIFO...");
00559             i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24);
00560             DEBUG_PRINT("Rewriting I2C master mode enabled because...I don't know\n");
00561             i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
00562             DEBUG_PRINT("Enabling and resetting DMP/FIFO...\n");
00563             i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8);
00564 
00565             DEBUG_PRINT("Writing final memory update 5/19 (function unknown)...\n");
00566             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00567             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00568             DEBUG_PRINT("Writing final memory update 6/19 (function unknown)...\n");
00569             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00570             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00571             DEBUG_PRINT("Writing final memory update 7/19 (function unknown)...\n");
00572             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00573             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00574             DEBUG_PRINT("Writing final memory update 8/19 (function unknown)...\n");
00575             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00576             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00577             DEBUG_PRINT("Writing final memory update 9/19 (function unknown)...\n");
00578             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00579             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00580             DEBUG_PRINT("Writing final memory update 10/19 (function unknown)...\n");
00581             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00582             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00583             DEBUG_PRINT("Writing final memory update 11/19 (function unknown)...\n");
00584             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00585             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00586             
00587             DEBUG_PRINT("Reading final memory update 12/19 (function unknown)...\n");
00588             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00589             readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00590 #ifdef DEBUG
00591                 DEBUG_PRINT("Read bytes: ");
00592                 for (j = 0; j < 4; j++) {
00593                     DEBUG_PRINTF("%u",dmpUpdate[3 + j]);
00594                     DEBUG_PRINT("\n");
00595                 }
00596                 DEBUG_PRINT("\n");
00597 #endif
00598 
00599             DEBUG_PRINT("Writing final memory update 13/19 (function unknown)...\n");
00600             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00601             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00602             DEBUG_PRINT("Writing final memory update 14/19 (function unknown)...\n");
00603             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00604             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00605             DEBUG_PRINT("Writing final memory update 15/19 (function unknown)...\n");
00606             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00607             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00608             DEBUG_PRINT("Writing final memory update 16/19 (function unknown)...\n");
00609             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00610             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00611             DEBUG_PRINT("Writing final memory update 17/19 (function unknown)...\n");
00612             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00613             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00614 
00615             DEBUG_PRINT("Waiting for FIFO count >= 46...\n");
00616             while ((fifoCount = getFIFOCount()) < 46);
00617             DEBUG_PRINT("Reading FIFO...\n");
00618             getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
00619             DEBUG_PRINT("Reading interrupt status...\n");
00620             getIntStatus();
00621 
00622             DEBUG_PRINT("Writing final memory update 18/19 (function unknown)...\n");
00623             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00624             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00625 
00626             DEBUG_PRINT("Waiting for FIRO count >= 48...\n");
00627             while ((fifoCount = getFIFOCount()) < 48);
00628             DEBUG_PRINT("Reading FIFO...");
00629             getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
00630             DEBUG_PRINT("Reading interrupt status...\n");
00631             getIntStatus();
00632             DEBUG_PRINT("Waiting for FIRO count >= 48...\n");
00633             while ((fifoCount = getFIFOCount()) < 48);
00634             DEBUG_PRINT("Reading FIFO...\n");
00635             getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
00636             DEBUG_PRINT("Reading interrupt status...\n");
00637             getIntStatus();
00638 
00639             DEBUG_PRINT("Writing final memory update 19/19 (function unknown)...\n");
00640             for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
00641             writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
00642 
00643             DEBUG_PRINT("Disabling DMP (you turn it on later)...\n");
00644             setDMPEnabled(false);
00645 
00646             DEBUG_PRINT("Setting up internal 48-byte (default) DMP packet buffer...\n");
00647             dmpPacketSize = 48;
00648             /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
00649 return 3; // TODO: proper error code for no memory
00650 }*/
00651 
00652             DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n");
00653             resetFIFO();
00654             getIntStatus();
00655         } else {
00656             DEBUG_PRINT("ERROR! DMP configuration verification failed.\n");
00657             return 2; // configuration block loading failed
00658         }
00659     } else {
00660         DEBUG_PRINT("ERROR! DMP code verification failed.\n");
00661         return 1; // main binary block loading failed
00662     }
00663     return 0; // success
00664 }
00665 
00666 bool MPU6050::dmpPacketAvailable() {
00667     return getFIFOCount() >= dmpGetFIFOPacketSize();
00668 }
00669 
00670 // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
00671 // uint8_t MPU6050::dmpGetFIFORate();
00672 // uint8_t MPU6050::dmpGetSampleStepSizeMS();
00673 // uint8_t MPU6050::dmpGetSampleFrequency();
00674 // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
00675 
00676 //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
00677 //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
00678 //uint8_t MPU6050::dmpRunFIFORateProcesses();
00679 
00680 // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
00681 // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
00682 // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00683 // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00684 // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
00685 // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
00686 // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
00687 // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
00688 // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
00689 // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
00690 // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
00691 // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
00692 
00693 uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
00694     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00695     if (packet == 0) packet = dmpPacketBuffer;
00696     data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]);
00697     data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]);
00698     data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]);
00699     return 0;
00700 }
00701 uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
00702     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00703     if (packet == 0) packet = dmpPacketBuffer;
00704     data[0] = (packet[34] << 8) + packet[35];
00705     data[1] = (packet[38] << 8) + packet[39];
00706     data[2] = (packet[42] << 8) + packet[43];
00707     return 0;
00708 }
00709 uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
00710     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00711     if (packet == 0) packet = dmpPacketBuffer;
00712     v -> x = (packet[34] << 8) + packet[35];
00713     v -> y = (packet[38] << 8) + packet[39];
00714     v -> z = (packet[42] << 8) + packet[43];
00715     return 0;
00716 }
00717 uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
00718     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00719     if (packet == 0) packet = dmpPacketBuffer;
00720     data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]);
00721     data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]);
00722     data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]);
00723     data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]);
00724     return 0;
00725 }
00726 uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
00727     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00728     if (packet == 0) packet = dmpPacketBuffer;
00729     data[0] = ((packet[0] << 8) + packet[1]);
00730     data[1] = ((packet[4] << 8) + packet[5]);
00731     data[2] = ((packet[8] << 8) + packet[9]);
00732     data[3] = ((packet[12] << 8) + packet[13]);
00733     return 0;
00734 }
00735 uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
00736     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00737     int16_t qI[4];
00738     uint8_t status = dmpGetQuaternion(qI, packet);
00739     if (status == 0) {
00740         q -> w = (float)qI[0] / 16384.0f;
00741         q -> x = (float)qI[1] / 16384.0f;
00742         q -> y = (float)qI[2] / 16384.0f;
00743         q -> z = (float)qI[3] / 16384.0f;
00744         return 0;
00745     }
00746     return status; // int16 return value, indicates error if this line is reached
00747 }
00748 // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
00749 // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
00750 uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
00751     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00752     if (packet == 0) packet = dmpPacketBuffer;
00753     data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]);
00754     data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]);
00755     data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]);
00756     return 0;
00757 }
00758 uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
00759     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00760     if (packet == 0) packet = dmpPacketBuffer;
00761     data[0] = (packet[16] << 8) + packet[17];
00762     data[1] = (packet[20] << 8) + packet[21];
00763     data[2] = (packet[24] << 8) + packet[25];
00764     return 0;
00765 }
00766 uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) {
00767     // TODO: accommodate different arrangements of sent data (ONLY default supported now)
00768     if (packet == 0) packet = dmpPacketBuffer;
00769     data[0] = (packet[28] << 8) + packet[29];
00770     data[1] = (packet[30] << 8) + packet[31];
00771     data[2] = (packet[32] << 8) + packet[33];
00772     return 0;
00773 }
00774 // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
00775 // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
00776 uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
00777     // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet)
00778     v -> x = vRaw -> x - gravity -> x*4096;
00779     v -> y = vRaw -> y - gravity -> y*4096;
00780     v -> z = vRaw -> z - gravity -> z*4096;
00781     return 0;
00782 }
00783 // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
00784 uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
00785     // rotate measured 3D acceleration vector into original state
00786     // frame of reference based on orientation quaternion
00787     memcpy(v, vReal, sizeof(VectorInt16));
00788     v -> rotate(q);
00789     return 0;
00790 }
00791 // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
00792 // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
00793 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
00794 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
00795 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
00796 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
00797     v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
00798     v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
00799     v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
00800     return 0;
00801 }
00802 // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
00803 // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
00804 // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
00805 // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
00806 
00807 uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
00808     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
00809     data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
00810     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
00811     return 0;
00812 }
00813 uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
00814     // yaw: (about Z axis)
00815     data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
00816     // pitch: (nose up/down, about Y axis)
00817     data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
00818     // roll: (tilt left/right, about X axis)
00819     data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
00820     return 0;
00821 }
00822 
00823 // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
00824 // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
00825 
00826 uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
00827     /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
00828 if (dmpData[k] < 0x10) Serial.print("0");
00829 Serial.print(dmpData[k], HEX);
00830 Serial.print(" ");
00831 }
00832 Serial.print("\n");*/
00833     //Serial.println((uint16_t)dmpPacketBuffer);
00834     return 0;
00835 }
00836 uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
00837     uint8_t status;
00838     uint8_t buf[dmpPacketSize];
00839     for (uint8_t i = 0; i < numPackets; i++) {
00840         // read packet from FIFO
00841         getFIFOBytes(buf, dmpPacketSize);
00842 
00843         // process packet
00844         if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
00845         
00846         // increment external process count variable, if supplied
00847         if (processed != 0) *processed++;
00848     }
00849     return 0;
00850 }
00851 
00852 // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
00853 
00854 // uint8_t MPU6050::dmpInitFIFOParam();
00855 // uint8_t MPU6050::dmpCloseFIFO();
00856 // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
00857 // uint8_t MPU6050::dmpDecodeQuantizedAccel();
00858 // uint32_t MPU6050::dmpGetGyroSumOfSquare();
00859 // uint32_t MPU6050::dmpGetAccelSumOfSquare();
00860 // void MPU6050::dmpOverrideQuaternion(long *q);
00861 uint16_t MPU6050::dmpGetFIFOPacketSize() {
00862     return dmpPacketSize;
00863 }
00864 
00865 #endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */