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