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