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