Chris Seto
MPU6050 issues
Fork of
MPU6050
by 
Shundo Kishi
Embed:
(wiki syntax)
Show/hide line numbers
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 "shared.h" 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(" %s \r\n", x); 00108 #define DEBUG_PRINTF(x, y) pc.printf(" %s \r\n", x); 00109 #define DEBUG_PRINTLN(x) pc.printf(" %s \r\n", x); 00110 #define DEBUG_PRINTLNF(x, y) pc.printf(" %s \r\n", x); 00111 #define F(x) x 00112 #else 00113 #define DEBUG_PRINT(x) 00114 #define DEBUG_PRINTF(x, y) 00115 #define DEBUG_PRINTLN(x) 00116 #define DEBUG_PRINTLNF(x, y) 00117 #endif 00118 00119 #define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] 00120 #define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] 00121 #define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] 00122 00123 /* ================================================================================================ * 00124 | Default MotionApps v2.0 42-byte FIFO packet structure: | 00125 | | 00126 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 00127 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 00128 | | 00129 | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 00130 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | 00131 * ================================================================================================ */ 00132 00133 // this block of memory gets written to the MPU on start-up, and it seems 00134 // to be volatile memory, so it has to be done each time (it only takes ~1 00135 // second though) 00136 const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 00137 // bank 0, 256 bytes 00138 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 00139 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 00140 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00141 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 00142 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 00143 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00144 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 00145 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 00146 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 00147 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 00148 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 00149 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 00150 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00151 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 00152 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 00153 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 00154 00155 // bank 1, 256 bytes 00156 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00157 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 00158 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 00159 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 00160 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 00161 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 00162 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 00163 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00164 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00165 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00166 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 00167 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 00168 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 00169 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 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, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 00172 00173 // bank 2, 256 bytes 00174 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00175 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 00176 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 00177 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00178 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00179 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00180 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00181 0x00, 0x00, 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, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00184 0x00, 0x00, 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, 0x40, 0x00, 0x00, 0x00, 00186 0x00, 0x1B, 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 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00189 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00190 00191 // bank 3, 256 bytes 00192 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, 00193 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 00194 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 00195 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 00196 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 00197 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 00198 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 00199 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 00200 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 00201 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 00202 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 00203 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 00204 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, 00205 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, 00206 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 00207 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 00208 00209 // bank 4, 256 bytes 00210 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 00211 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 00212 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 00213 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 00214 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 00215 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 00216 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 00217 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 00218 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 00219 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 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, 0x93, 0xA3, 0x4D, 00222 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 00223 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 00224 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 00225 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 00226 00227 // bank 5, 256 bytes 00228 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 00229 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 00230 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 00231 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 00232 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 00233 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 00234 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 00235 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 00236 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, 00237 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, 00238 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 00239 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 00240 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 00241 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, 00242 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, 00243 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, 00244 00245 // bank 6, 256 bytes 00246 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, 00247 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, 00248 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, 00249 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, 00250 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, 00251 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, 00252 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, 00253 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, 00254 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 00255 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, 00256 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, 00257 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, 00258 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, 00259 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, 00260 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, 00261 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, 00262 00263 // bank 7, 138 bytes (remainder) 00264 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, 00265 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, 00266 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, 00267 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 00268 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, 00269 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 00270 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 0xA3, 00271 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 00272 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 00273 }; 00274 00275 // thanks to Noah Zerkin for piecing this stuff together! 00276 const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { 00277 // BANK OFFSET LENGTH [DATA] 00278 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 00279 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 00280 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration 00281 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration 00282 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 00283 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration 00284 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration 00285 0x00, 0x6C, 0x02, 0x20, 0x00, // D_0_108 inv_set_accel_calibration 00286 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 00287 0x02, 0x44, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 00288 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 00289 0x02, 0x4C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_10 00290 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 00291 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 00292 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 00293 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 00294 0x02, 0xBC, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 00295 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 00296 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 00297 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 00298 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 00299 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone 00300 // SPECIAL 0x01 = enable interrupts 00301 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION 00302 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt 00303 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 00304 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer 00305 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 00306 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 00307 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 00308 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate 00309 00310 // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, 00311 // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. 00312 // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) 00313 00314 // It is important to make sure the host processor can keep up with reading and processing 00315 // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. 00316 }; 00317 00318 const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { 00319 0x01, 0xB2, 0x02, 0xFF, 0xFF, 00320 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, 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 0x01, 0x62, 0x02, 0x00, 0x00, 00325 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 00326 }; 00327 00328 uint8_t MPU6050::dmpInitialize() { 00329 // reset device 00330 DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); 00331 reset(); 00332 //delay(30); // wait after reset 00333 wait_ms(30); 00334 00335 // enable sleep mode and wake cycle 00336 /*Serial.println(F("Enabling sleep mode...")); 00337 setSleepEnabled(true); 00338 Serial.println(F("Enabling wake cycle...")); 00339 setWakeCycleEnabled(true);*/ 00340 00341 // disable sleep mode 00342 DEBUG_PRINTLN(F("Disabling sleep mode...")); 00343 setSleepEnabled(false); 00344 00345 // get MPU hardware revision 00346 DEBUG_PRINTLN(F("Selecting user bank 16...")); 00347 setMemoryBank(0x10, true, true); 00348 DEBUG_PRINTLN(F("Selecting memory byte 6...")); 00349 setMemoryStartAddress(0x06); 00350 DEBUG_PRINTLN(F("Checking hardware revision...")); 00351 uint8_t hwRevision = readMemoryByte(); 00352 pc.printf("Revision @ user[16][6] = %d\r\n", hwRevision); 00353 DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 00354 setMemoryBank(0, false, false); 00355 00356 // check OTP bank valid 00357 DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 00358 uint8_t otpValid = getOTPBankValid(); 00359 DEBUG_PRINT(F("OTP bank is ")); 00360 DEBUG_PRINTLN((otpValid ? F("valid!") : F("invalid!"))); 00361 00362 // get X/Y/Z gyro offsets 00363 DEBUG_PRINTLN(F("Reading gyro offset TC values...")); 00364 //int8_t xgOffsetTC = getXGyroOffsetTC(); 00365 //int8_t ygOffsetTC = getYGyroOffsetTC(); 00366 //int8_t zgOffsetTC = getZGyroOffsetTC(); 00367 //DEBUG_PRINT(F("X gyro offset = ")); 00368 //DEBUG_PRINTLN(xgOffset); 00369 //DEBUG_PRINT(F("Y gyro offset = ")); 00370 //DEBUG_PRINTLN(ygOffset); 00371 //DEBUG_PRINT(F("Z gyro offset = ")); 00372 //DEBUG_PRINTLN(zgOffset); 00373 00374 // setup weird slave stuff (?) 00375 DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); 00376 setSlaveAddress(0, 0x7F); 00377 DEBUG_PRINTLN(F("Disabling I2C Master mode...")); 00378 setI2CMasterModeEnabled(false); 00379 DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); 00380 setSlaveAddress(0, 0x68); 00381 DEBUG_PRINTLN(F("Resetting I2C Master control...")); 00382 resetI2CMaster(); 00383 //delay(20); 00384 wait_ms(20); 00385 00386 // load DMP code into memory banks 00387 DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 00388 pc.printf("%d bytes)\r\n", MPU6050_DMP_CODE_SIZE); 00389 00390 if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) 00391 { 00392 DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 00393 00394 // write DMP configuration 00395 DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 00396 DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); 00397 DEBUG_PRINTLN(F(" bytes in config def)")); 00398 if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { 00399 DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 00400 00401 DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 00402 setClockSource(MPU6050_CLOCK_PLL_ZGYRO); 00403 00404 DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 00405 setIntEnabled(0x12); 00406 00407 DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 00408 setRate(4); // 1khz / (1 + 4) = 200 Hz 00409 00410 DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 00411 setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); 00412 00413 DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 00414 setDLPFMode(MPU6050_DLPF_BW_42); 00415 00416 DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 00417 setFullScaleGyroRange(MPU6050_GYRO_FS_2000); 00418 00419 DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 00420 setDMPConfig1(0x03); 00421 setDMPConfig2(0x00); 00422 00423 DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 00424 setOTPBankValid(false); 00425 00426 DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); 00427 //setXGyroOffsetTC(xgOffsetTC); 00428 //setYGyroOffsetTC(ygOffsetTC); 00429 //setZGyroOffsetTC(zgOffsetTC); 00430 00431 //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 00432 //setXGyroOffset(0); 00433 //setYGyroOffset(0); 00434 //setZGyroOffset(0); 00435 00436 DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 00437 uint8_t dmpUpdate[16], j; 00438 uint16_t pos = 0; 00439 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00440 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00441 00442 DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); 00443 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00444 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00445 00446 DEBUG_PRINTLN(F("Resetting FIFO...")); 00447 resetFIFO(); 00448 00449 DEBUG_PRINTLN(F("Reading FIFO count...")); 00450 uint16_t fifoCount = getFIFOCount(); 00451 uint8_t fifoBuffer[128]; 00452 00453 DEBUG_PRINT(F("Current FIFO count=")); 00454 DEBUG_PRINTLN(fifoCount); 00455 getFIFOBytes(fifoBuffer, fifoCount); 00456 00457 DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 00458 setMotionDetectionThreshold(2); 00459 00460 DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 00461 setZeroMotionDetectionThreshold(156); 00462 00463 DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 00464 setMotionDetectionDuration(80); 00465 00466 DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 00467 setZeroMotionDetectionDuration(0); 00468 00469 DEBUG_PRINTLN(F("Resetting FIFO...")); 00470 resetFIFO(); 00471 00472 DEBUG_PRINTLN(F("Enabling FIFO...")); 00473 setFIFOEnabled(true); 00474 00475 DEBUG_PRINTLN(F("Enabling DMP...")); 00476 setDMPEnabled(true); 00477 00478 DEBUG_PRINTLN(F("Resetting DMP...")); 00479 resetDMP(); 00480 00481 DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); 00482 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00483 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00484 00485 DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); 00486 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00487 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00488 00489 DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); 00490 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00491 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00492 00493 DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 00494 while ((fifoCount = getFIFOCount()) < 3); 00495 00496 DEBUG_PRINT(F("Current FIFO count=")); 00497 DEBUG_PRINTLN(fifoCount); 00498 DEBUG_PRINTLN(F("Reading FIFO data...")); 00499 getFIFOBytes(fifoBuffer, fifoCount); 00500 00501 DEBUG_PRINTLN(F("Reading interrupt status...")); 00502 uint8_t mpuIntStatus = getIntStatus(); 00503 00504 DEBUG_PRINT(F("Current interrupt status=")); 00505 DEBUG_PRINTLNF(mpuIntStatus, HEX); 00506 00507 DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); 00508 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00509 readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00510 00511 DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); 00512 while ((fifoCount = getFIFOCount()) < 3); 00513 00514 DEBUG_PRINT(F("Current FIFO count=")); 00515 DEBUG_PRINTLN(fifoCount); 00516 00517 DEBUG_PRINTLN(F("Reading FIFO data...")); 00518 getFIFOBytes(fifoBuffer, fifoCount); 00519 00520 DEBUG_PRINTLN(F("Reading interrupt status...")); 00521 mpuIntStatus = getIntStatus(); 00522 00523 DEBUG_PRINT(F("Current interrupt status=")); 00524 DEBUG_PRINTLNF(mpuIntStatus, HEX); 00525 00526 DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); 00527 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00528 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00529 00530 DEBUG_PRINTLN(F("DMP is good to go! Finally.")); 00531 00532 DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 00533 setDMPEnabled(false); 00534 00535 DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); 00536 dmpPacketSize = 42; 00537 /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 00538 return 3; // TODO: proper error code for no memory 00539 }*/ 00540 00541 DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 00542 resetFIFO(); 00543 getIntStatus(); 00544 } else { 00545 DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 00546 return 2; // configuration block loading failed 00547 } 00548 } else { 00549 DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 00550 return 1; // main binary block loading failed 00551 } 00552 return 0; // success 00553 } 00554 00555 bool MPU6050::dmpPacketAvailable() { 00556 return getFIFOCount() >= dmpGetFIFOPacketSize(); 00557 } 00558 00559 // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); 00560 // uint8_t MPU6050::dmpGetFIFORate(); 00561 // uint8_t MPU6050::dmpGetSampleStepSizeMS(); 00562 // uint8_t MPU6050::dmpGetSampleFrequency(); 00563 // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); 00564 00565 //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 00566 //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); 00567 //uint8_t MPU6050::dmpRunFIFORateProcesses(); 00568 00569 // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); 00570 // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 00571 // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00572 // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00573 // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 00574 // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 00575 // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 00576 // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 00577 // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 00578 // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); 00579 // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00580 // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 00581 00582 uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { 00583 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00584 if (packet == 0) packet = dmpPacketBuffer; 00585 data[0] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); 00586 data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); 00587 data[2] = ((packet[36] << 24) + (packet[37] << 16) + (packet[38] << 8) + packet[39]); 00588 return 0; 00589 } 00590 uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { 00591 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00592 if (packet == 0) packet = dmpPacketBuffer; 00593 data[0] = (packet[28] << 8) + packet[29]; 00594 data[1] = (packet[32] << 8) + packet[33]; 00595 data[2] = (packet[36] << 8) + packet[37]; 00596 return 0; 00597 } 00598 uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 00599 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00600 if (packet == 0) packet = dmpPacketBuffer; 00601 v -> x = (packet[28] << 8) + packet[29]; 00602 v -> y = (packet[32] << 8) + packet[33]; 00603 v -> z = (packet[36] << 8) + packet[37]; 00604 return 0; 00605 } 00606 uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 00607 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00608 if (packet == 0) packet = dmpPacketBuffer; 00609 data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 00610 data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 00611 data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 00612 data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 00613 return 0; 00614 } 00615 uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 00616 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00617 if (packet == 0) packet = dmpPacketBuffer; 00618 data[0] = ((packet[0] << 8) + packet[1]); 00619 data[1] = ((packet[4] << 8) + packet[5]); 00620 data[2] = ((packet[8] << 8) + packet[9]); 00621 data[3] = ((packet[12] << 8) + packet[13]); 00622 return 0; 00623 } 00624 uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 00625 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00626 int16_t qI[4]; 00627 uint8_t status = dmpGetQuaternion(qI, packet); 00628 if (status == 0) { 00629 q -> w = (float)qI[0] / 16384.0f; 00630 q -> x = (float)qI[1] / 16384.0f; 00631 q -> y = (float)qI[2] / 16384.0f; 00632 q -> z = (float)qI[3] / 16384.0f; 00633 return 0; 00634 } 00635 return status; // int16 return value, indicates error if this line is reached 00636 } 00637 // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 00638 // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 00639 uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { 00640 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00641 if (packet == 0) packet = dmpPacketBuffer; 00642 data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 00643 data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 00644 data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 00645 return 0; 00646 } 00647 uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { 00648 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00649 if (packet == 0) packet = dmpPacketBuffer; 00650 data[0] = (packet[16] << 8) + packet[17]; 00651 data[1] = (packet[20] << 8) + packet[21]; 00652 data[2] = (packet[24] << 8) + packet[25]; 00653 return 0; 00654 } 00655 // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); 00656 // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); 00657 uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 00658 // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) 00659 v -> x = vRaw -> x - gravity -> x*8192; 00660 v -> y = vRaw -> y - gravity -> y*8192; 00661 v -> z = vRaw -> z - gravity -> z*8192; 00662 return 0; 00663 } 00664 // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 00665 uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 00666 // rotate measured 3D acceleration vector into original state 00667 // frame of reference based on orientation quaternion 00668 memcpy(v, vReal, sizeof(VectorInt16)); 00669 v -> rotate(q); 00670 return 0; 00671 } 00672 // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 00673 // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); 00674 // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); 00675 // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); 00676 // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); 00677 uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { 00678 v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 00679 v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 00680 v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 00681 return 0; 00682 } 00683 // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 00684 // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 00685 // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 00686 // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); 00687 00688 uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { 00689 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 00690 data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 00691 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 00692 return 0; 00693 } 00694 uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 00695 // yaw: (about Z axis) 00696 data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 00697 // pitch: (nose up/down, about Y axis) 00698 data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 00699 // roll: (tilt left/right, about X axis) 00700 data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 00701 return 0; 00702 } 00703 00704 // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); 00705 // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 00706 00707 uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { 00708 /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 00709 if (dmpData[k] < 0x10) Serial.print("0"); 00710 Serial.print(dmpData[k], HEX); 00711 Serial.print(" "); 00712 } 00713 Serial.print("\n");*/ 00714 //Serial.println((uint16_t)dmpPacketBuffer); 00715 return 0; 00716 } 00717 uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 00718 uint8_t status; 00719 uint8_t buf[dmpPacketSize]; 00720 for (uint8_t i = 0; i < numPackets; i++) { 00721 // read packet from FIFO 00722 getFIFOBytes(buf, dmpPacketSize); 00723 00724 // process packet 00725 if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 00726 00727 // increment external process count variable, if supplied 00728 if (processed != 0) *processed++; 00729 } 00730 return 0; 00731 } 00732 00733 // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); 00734 00735 // uint8_t MPU6050::dmpInitFIFOParam(); 00736 // uint8_t MPU6050::dmpCloseFIFO(); 00737 // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); 00738 // uint8_t MPU6050::dmpDecodeQuantizedAccel(); 00739 // uint32_t MPU6050::dmpGetGyroSumOfSquare(); 00740 // uint32_t MPU6050::dmpGetAccelSumOfSquare(); 00741 // void MPU6050::dmpOverrideQuaternion(long *q); 00742 uint16_t MPU6050::dmpGetFIFOPacketSize() { 00743 return dmpPacketSize; 00744 } 00745 00746 #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */
Generated on Tue Jul 12 2022 20:58:39 by
1.7.2