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