Shundo Kishi
9Axis IMU MPU9150 's library. This project is ported from this Arduino's project https://github.com/jrowberg/i2cdevlib/tree/master/Arduino/MPU9150. Connect pinName 27 to SCL, PinName 28 to SDA, GND to GND, and VOUT to VCC to try this library. The example is here
Dependencies: ArduinoSerial I2Cdev
Embed:
(wiki syntax)
Show/hide line numbers
MPU9150_9Axis_MotionApps41.h
00001 #ifndef _MPU9150_9AXIS_MOTIONAPPS41_H_ 00002 #define _MPU9150_9AXIS_MOTIONAPPS41_H_ 00003 00004 #include "I2Cdev.h" 00005 #include "helper_3dmath.h" 00006 00007 // MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board 00008 #define MPU9150_INCLUDE_DMP_MOTIONAPPS41 00009 00010 #include "MPU9150.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 // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. 00058 // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) 00059 // after moving string constants to flash memory storage using the F() 00060 // compiler macro (Arduino IDE 1.0+ required). 00061 00062 //#define DEBUG 00063 #ifdef DEBUG 00064 #include "ArduinoSerial.h" 00065 ArduinoSerial arduinoSerial; 00066 #define DEBUG_PRINT(x) arduinoSerial.print(x) 00067 #define DEBUG_PRINTF(x, y) arduinoSerial.print(x, y) 00068 #define DEBUG_PRINTLN(x) arduinoSerial.println(x) 00069 #define DEBUG_PRINTLNF(x, y) arduinoSerial.println(x, y) 00070 #else 00071 #define DEBUG_PRINT(x) 00072 #define DEBUG_PRINTF(x, y) 00073 #define DEBUG_PRINTLN(x) 00074 #define DEBUG_PRINTLNF(x, y) 00075 #endif 00076 00077 #define MPU9150_DMP_CODE_SIZE 1962 // dmpMemory[] 00078 #define MPU9150_DMP_CONFIG_SIZE 232 // dmpConfig[] 00079 #define MPU9150_DMP_UPDATES_SIZE 140 // dmpUpdates[] 00080 00081 /* ================================================================================================ * 00082 | Default MotionApps v4.1 48-byte FIFO packet structure: | 00083 | | 00084 | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | 00085 | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | 00086 | | 00087 | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | 00088 | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | 00089 * ================================================================================================ */ 00090 00091 // this block of memory gets written to the MPU on start-up, and it seems 00092 // to be volatile memory, so it has to be done each time (it only takes ~1 00093 // second though) 00094 const prog_uchar dmpMemory[MPU9150_DMP_CODE_SIZE] PROGMEM = { 00095 // bank 0, 256 bytes 00096 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, 00097 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, 00098 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00099 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, 00100 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, 00101 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00102 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, 00103 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, 00104 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, 00105 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, 00106 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, 00107 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, 00108 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00109 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, 00110 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, 00111 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, 00112 00113 // bank 1, 256 bytes 00114 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00115 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, 00116 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, 00117 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, 00118 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, 00119 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, 00120 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, 00121 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00122 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00123 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00124 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, 00125 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, 00126 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, 00127 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00128 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00129 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, 00130 00131 // bank 2, 256 bytes 00132 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00133 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, 00134 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, 00135 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00136 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00137 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00138 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00139 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00140 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00141 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00142 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00143 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 00144 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00145 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00146 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, 00147 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00148 00149 // bank 3, 256 bytes 00150 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, 00151 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 00152 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 00153 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 00154 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 00155 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 00156 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 00157 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 00158 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 00159 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 00160 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 00161 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 00162 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 00163 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, 00164 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, 00165 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, 00166 00167 // bank 4, 256 bytes 00168 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, 00169 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, 00170 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, 00171 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, 00172 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, 00173 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, 00174 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, 00175 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, 00176 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, 00177 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, 00178 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, 00179 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, 00180 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 00181 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, 00182 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, 00183 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, 00184 00185 // bank 5, 256 bytes 00186 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, 00187 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, 00188 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, 00189 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, 00190 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, 00191 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, 00192 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, 00193 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, 00194 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 00195 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, 00196 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, 00197 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 00198 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, 00199 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, 00200 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 00201 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, 00202 00203 // bank 6, 256 bytes 00204 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, 00205 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 00206 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 00207 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 00208 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 00209 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 00210 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 00211 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 00212 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 00213 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 00214 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 00215 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 00216 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 00217 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 00218 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 00219 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 00220 00221 // bank 7, 170 bytes (remainder) 00222 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 00223 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 00224 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 00225 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 00226 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 00227 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, 00228 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, 00229 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 00230 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, 00231 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, 00232 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF 00233 }; 00234 00235 const prog_uchar dmpConfig[MPU9150_DMP_CONFIG_SIZE] PROGMEM = { 00236 // BANK OFFSET LENGTH [DATA] 00237 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? 00238 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration 00239 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration 00240 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration 00241 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration 00242 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration 00243 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration 00244 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration 00245 00246 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration 00247 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 00248 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 00249 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 00250 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 00251 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 00252 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 00253 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 00254 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 00255 00256 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel 00257 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors 00258 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion 00259 0x00, 0xA3, 0x01, 0x00, // ? 00260 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update 00261 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion 00262 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer 00263 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro 00264 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo 00265 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? 00266 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? 00267 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? 00268 // SPECIAL 0x01 = enable interrupts 00269 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION 00270 0x07, 0xA7, 0x01, 0xFE, // ? 00271 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? 00272 0x07, 0x67, 0x01, 0x9A, // ? 00273 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo 00274 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo 00275 0x02, 0x16, 0x02, 0x00, 0x03 // 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 prog_uchar dmpUpdates[MPU9150_DMP_UPDATES_SIZE] PROGMEM = { 00286 0x01, 0xB2, 0x02, 0xFF, 0xF5, 00287 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, 00288 0x00, 0xA3, 0x01, 0x00, 00289 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, 00290 0x01, 0x6A, 0x02, 0x06, 0x00, 00291 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00292 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 00293 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 00294 0x01, 0x08, 0x02, 0x01, 0x20, 00295 0x01, 0x0A, 0x02, 0x00, 0x4E, 00296 0x01, 0x02, 0x02, 0xFE, 0xB3, 00297 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ 00298 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, 00299 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, 00300 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, 00301 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, 00302 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, 00303 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, 00304 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 00305 }; 00306 00307 uint8_t MPU9150::dmpInitialize() { 00308 // reset device 00309 DEBUG_PRINTLN(F("\n\nResetting MPU9150...")); 00310 reset(); 00311 wait_ms(30); // wait after reset 00312 00313 // disable sleep mode 00314 DEBUG_PRINTLN(F("Disabling sleep mode...")); 00315 setSleepEnabled(false); 00316 00317 // get MPU product ID 00318 DEBUG_PRINTLN(F("Getting product ID...")); 00319 //uint8_t productID = 0; //getProductID(); 00320 DEBUG_PRINT(F("Product ID = ")); 00321 DEBUG_PRINT(productID); 00322 00323 // get MPU hardware revision 00324 DEBUG_PRINTLN(F("Selecting user bank 16...")); 00325 setMemoryBank(0x10, true, true); 00326 DEBUG_PRINTLN(F("Selecting memory byte 6...")); 00327 setMemoryStartAddress(0x06); 00328 DEBUG_PRINTLN(F("Checking hardware revision...")); 00329 uint8_t hwRevision = readMemoryByte(); 00330 DEBUG_PRINT(F("Revision @ user[16][6] = ")); 00331 DEBUG_PRINTLNF(hwRevision, HEX); 00332 DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); 00333 setMemoryBank(0, false, false); 00334 00335 // check OTP bank valid 00336 DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); 00337 uint8_t otpValid = getOTPBankValid(); 00338 DEBUG_PRINT(F("OTP bank is ")); 00339 DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); 00340 00341 // get X/Y/Z gyro offsets 00342 DEBUG_PRINTLN(F("Reading gyro offset values...")); 00343 int8_t xgOffset = getXGyroOffset(); 00344 int8_t ygOffset = getYGyroOffset(); 00345 int8_t zgOffset = getZGyroOffset(); 00346 DEBUG_PRINT(F("X gyro offset = ")); 00347 DEBUG_PRINTLN(xgOffset); 00348 DEBUG_PRINT(F("Y gyro offset = ")); 00349 DEBUG_PRINTLN(ygOffset); 00350 DEBUG_PRINT(F("Z gyro offset = ")); 00351 DEBUG_PRINTLN(zgOffset); 00352 00353 I2Cdev::readByte(devAddr, MPU9150_RA_USER_CTRL, buffer); // ? 00354 00355 DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); 00356 I2Cdev::writeByte(devAddr, MPU9150_RA_INT_PIN_CFG, 0x32); 00357 00358 // enable MPU AUX I2C bypass mode 00359 //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); 00360 //setI2CBypassEnabled(true); 00361 00362 DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); 00363 //mag -> setMode(0); 00364 I2Cdev::writeByte(0x0E, 0x0A, 0x00); 00365 00366 DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); 00367 //mag -> setMode(0x0F); 00368 I2Cdev::writeByte(0x0E, 0x0A, 0x0F); 00369 00370 DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); 00371 int8_t asax, asay, asaz; 00372 //mag -> getAdjustment(&asax, &asay, &asaz); 00373 I2Cdev::readBytes(0x0E, 0x10, 3, buffer); 00374 asax = (int8_t)buffer[0]; 00375 asay = (int8_t)buffer[1]; 00376 asaz = (int8_t)buffer[2]; 00377 DEBUG_PRINT(F("Adjustment X/Y/Z = ")); 00378 DEBUG_PRINT(asax); 00379 DEBUG_PRINT(F(" / ")); 00380 DEBUG_PRINT(asay); 00381 DEBUG_PRINT(F(" / ")); 00382 DEBUG_PRINTLN(asaz); 00383 00384 DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); 00385 //mag -> setMode(0); 00386 I2Cdev::writeByte(0x0E, 0x0A, 0x00); 00387 00388 // load DMP code into memory banks 00389 DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); 00390 DEBUG_PRINT(MPU9150_DMP_CODE_SIZE); 00391 DEBUG_PRINTLN(F(" bytes)")); 00392 if (writeProgMemoryBlock(dmpMemory, MPU9150_DMP_CODE_SIZE)) { 00393 DEBUG_PRINTLN(F("Success! DMP code written and verified.")); 00394 00395 DEBUG_PRINTLN(F("Configuring DMP and related settings...")); 00396 00397 // write DMP configuration 00398 DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); 00399 DEBUG_PRINT(MPU9150_DMP_CONFIG_SIZE); 00400 DEBUG_PRINTLN(F(" bytes in config def)")); 00401 if (writeProgDMPConfigurationSet(dmpConfig, MPU9150_DMP_CONFIG_SIZE)) { 00402 DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); 00403 00404 DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); 00405 setIntEnabled(0x12); 00406 00407 DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); 00408 setRate(4); // 1khz / (1 + 4) = 200 Hz 00409 00410 DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); 00411 setClockSource(MPU9150_CLOCK_PLL_ZGYRO); 00412 00413 DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); 00414 setDLPFMode(MPU9150_DLPF_BW_42); 00415 00416 DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); 00417 setExternalFrameSync(MPU9150_EXT_SYNC_TEMP_OUT_L); 00418 00419 DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); 00420 setFullScaleGyroRange(MPU9150_GYRO_FS_2000); 00421 00422 DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); 00423 setDMPConfig1(0x03); 00424 setDMPConfig2(0x00); 00425 00426 DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); 00427 setOTPBankValid(false); 00428 00429 DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values...")); 00430 setXGyroOffsetTC(xgOffset); 00431 setYGyroOffsetTC(ygOffset); 00432 setZGyroOffsetTC(zgOffset); 00433 00434 DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); 00435 setXGyroOffset(0); 00436 setYGyroOffset(0); 00437 setZGyroOffset(0); 00438 00439 DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)...")); 00440 uint8_t dmpUpdate[16], j; 00441 uint16_t pos = 0; 00442 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00443 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00444 00445 DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)...")); 00446 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00447 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00448 00449 DEBUG_PRINTLN(F("Resetting FIFO...")); 00450 resetFIFO(); 00451 00452 DEBUG_PRINTLN(F("Reading FIFO count...")); 00453 uint8_t fifoCount = getFIFOCount(); 00454 00455 DEBUG_PRINT(F("Current FIFO count=")); 00456 DEBUG_PRINTLN(fifoCount); 00457 uint8_t fifoBuffer[128]; 00458 //getFIFOBytes(fifoBuffer, fifoCount); 00459 00460 DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)...")); 00461 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00462 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00463 00464 DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)...")); 00465 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00466 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00467 00468 DEBUG_PRINTLN(F("Disabling all standby flags...")); 00469 I2Cdev::writeByte(0x68, MPU9150_RA_PWR_MGMT_2, 0x00); 00470 00471 DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g...")); 00472 I2Cdev::writeByte(0x68, MPU9150_RA_ACCEL_CONFIG, 0x00); 00473 00474 DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); 00475 setMotionDetectionThreshold(2); 00476 00477 DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); 00478 setZeroMotionDetectionThreshold(156); 00479 00480 DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); 00481 setMotionDetectionDuration(80); 00482 00483 DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); 00484 setZeroMotionDetectionDuration(0); 00485 00486 DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode...")); 00487 //mag -> setMode(1); 00488 I2Cdev::writeByte(0x0E, 0x0A, 0x01); 00489 00490 // setup AK8975 (0x0E) as Slave 0 in read mode 00491 DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); 00492 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_ADDR, 0x8E); 00493 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_REG, 0x01); 00494 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_CTRL, 0xDA); 00495 00496 // setup AK8975 (0x0E) as Slave 2 in write mode 00497 DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); 00498 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_ADDR, 0x0E); 00499 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_REG, 0x0A); 00500 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_CTRL, 0x81); 00501 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_DO, 0x01); 00502 00503 // setup I2C timing/delay control 00504 DEBUG_PRINTLN(F("Setting up slave access delay...")); 00505 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV4_CTRL, 0x18); 00506 I2Cdev::writeByte(0x68, MPU9150_RA_I2C_MST_DELAY_CTRL, 0x05); 00507 00508 // enable interrupts 00509 DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); 00510 I2Cdev::writeByte(0x68, MPU9150_RA_INT_PIN_CFG, 0x00); 00511 00512 // enable I2C master mode and reset DMP/FIFO 00513 DEBUG_PRINTLN(F("Enabling I2C master mode...")); 00514 I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20); 00515 DEBUG_PRINTLN(F("Resetting FIFO...")); 00516 I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x24); 00517 DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); 00518 I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20); 00519 DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); 00520 I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0xE8); 00521 00522 DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); 00523 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00524 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00525 DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); 00526 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00527 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00528 DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); 00529 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00530 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00531 DEBUG_PRINTLN(F("Writing final memory update 8/19 (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 DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); 00535 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00536 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00537 DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); 00538 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00539 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00540 DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); 00541 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00542 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00543 00544 DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); 00545 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00546 readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00547 #ifdef DEBUG 00548 DEBUG_PRINT(F("Read bytes: ")); 00549 for (j = 0; j < 4; j++) { 00550 DEBUG_PRINTF(dmpUpdate[3 + j], HEX); 00551 DEBUG_PRINT(" "); 00552 } 00553 DEBUG_PRINTLN(""); 00554 #endif 00555 00556 DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); 00557 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00558 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00559 DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); 00560 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00561 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00562 DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); 00563 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00564 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00565 DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); 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_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); 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 00572 DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); 00573 while ((fifoCount = getFIFOCount()) < 46); 00574 DEBUG_PRINTLN(F("Reading FIFO...")); 00575 getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 00576 DEBUG_PRINTLN(F("Reading interrupt status...")); 00577 getIntStatus(); 00578 00579 DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); 00580 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00581 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00582 00583 DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); 00584 while ((fifoCount = getFIFOCount()) < 48); 00585 DEBUG_PRINTLN(F("Reading FIFO...")); 00586 getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 00587 DEBUG_PRINTLN(F("Reading interrupt status...")); 00588 getIntStatus(); 00589 DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); 00590 while ((fifoCount = getFIFOCount()) < 48); 00591 DEBUG_PRINTLN(F("Reading FIFO...")); 00592 getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes 00593 DEBUG_PRINTLN(F("Reading interrupt status...")); 00594 getIntStatus(); 00595 00596 DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); 00597 for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); 00598 writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); 00599 00600 DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); 00601 setDMPEnabled(false); 00602 00603 DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); 00604 dmpPacketSize = 48; 00605 /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { 00606 return 3; // TODO: proper error code for no memory 00607 }*/ 00608 00609 DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); 00610 resetFIFO(); 00611 getIntStatus(); 00612 } else { 00613 DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); 00614 return 2; // configuration block loading failed 00615 } 00616 } else { 00617 DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); 00618 return 1; // main binary block loading failed 00619 } 00620 return 0; // success 00621 } 00622 00623 bool MPU9150::dmpPacketAvailable() { 00624 return getFIFOCount() >= dmpGetFIFOPacketSize(); 00625 } 00626 00627 // uint8_t MPU9150::dmpSetFIFORate(uint8_t fifoRate); 00628 // uint8_t MPU9150::dmpGetFIFORate(); 00629 // uint8_t MPU9150::dmpGetSampleStepSizeMS(); 00630 // uint8_t MPU9150::dmpGetSampleFrequency(); 00631 // int32_t MPU9150::dmpDecodeTemperature(int8_t tempReg); 00632 00633 //uint8_t MPU9150::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); 00634 //uint8_t MPU9150::dmpUnregisterFIFORateProcess(inv_obj_func func); 00635 //uint8_t MPU9150::dmpRunFIFORateProcesses(); 00636 00637 // uint8_t MPU9150::dmpSendQuaternion(uint_fast16_t accuracy); 00638 // uint8_t MPU9150::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); 00639 // uint8_t MPU9150::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00640 // uint8_t MPU9150::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00641 // uint8_t MPU9150::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); 00642 // uint8_t MPU9150::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); 00643 // uint8_t MPU9150::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 00644 // uint8_t MPU9150::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); 00645 // uint8_t MPU9150::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); 00646 // uint8_t MPU9150::dmpSendPacketNumber(uint_fast16_t accuracy); 00647 // uint8_t MPU9150::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); 00648 // uint8_t MPU9150::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); 00649 00650 uint8_t MPU9150::dmpGetAccel(int32_t *data, const uint8_t* packet) { 00651 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00652 if (packet == 0) packet = dmpPacketBuffer; 00653 data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]); 00654 data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]); 00655 data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]); 00656 return 0; 00657 } 00658 uint8_t MPU9150::dmpGetAccel(int16_t *data, const uint8_t* packet) { 00659 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00660 if (packet == 0) packet = dmpPacketBuffer; 00661 data[0] = (packet[34] << 8) + packet[35]; 00662 data[1] = (packet[38] << 8) + packet[39]; 00663 data[2] = (packet[42] << 8) + packet[43]; 00664 return 0; 00665 } 00666 uint8_t MPU9150::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { 00667 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00668 if (packet == 0) packet = dmpPacketBuffer; 00669 v -> x = (packet[34] << 8) + packet[35]; 00670 v -> y = (packet[38] << 8) + packet[39]; 00671 v -> z = (packet[42] << 8) + packet[43]; 00672 return 0; 00673 } 00674 uint8_t MPU9150::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { 00675 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00676 if (packet == 0) packet = dmpPacketBuffer; 00677 data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); 00678 data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); 00679 data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); 00680 data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); 00681 return 0; 00682 } 00683 uint8_t MPU9150::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { 00684 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00685 if (packet == 0) packet = dmpPacketBuffer; 00686 data[0] = ((packet[0] << 8) + packet[1]); 00687 data[1] = ((packet[4] << 8) + packet[5]); 00688 data[2] = ((packet[8] << 8) + packet[9]); 00689 data[3] = ((packet[12] << 8) + packet[13]); 00690 return 0; 00691 } 00692 uint8_t MPU9150::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { 00693 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00694 int16_t qI[4]; 00695 uint8_t status = dmpGetQuaternion(qI, packet); 00696 if (status == 0) { 00697 q -> w = (float)qI[0] / 16384.0f; 00698 q -> x = (float)qI[1] / 16384.0f; 00699 q -> y = (float)qI[2] / 16384.0f; 00700 q -> z = (float)qI[3] / 16384.0f; 00701 return 0; 00702 } 00703 return status; // int16 return value, indicates error if this line is reached 00704 } 00705 // uint8_t MPU9150::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); 00706 // uint8_t MPU9150::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); 00707 uint8_t MPU9150::dmpGetGyro(int32_t *data, const uint8_t* packet) { 00708 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00709 if (packet == 0) packet = dmpPacketBuffer; 00710 data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); 00711 data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); 00712 data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); 00713 return 0; 00714 } 00715 uint8_t MPU9150::dmpGetGyro(int16_t *data, const uint8_t* packet) { 00716 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00717 if (packet == 0) packet = dmpPacketBuffer; 00718 data[0] = (packet[16] << 8) + packet[17]; 00719 data[1] = (packet[20] << 8) + packet[21]; 00720 data[2] = (packet[24] << 8) + packet[25]; 00721 return 0; 00722 } 00723 uint8_t MPU9150::dmpGetMag(int16_t *data, const uint8_t* packet) { 00724 // TODO: accommodate different arrangements of sent data (ONLY default supported now) 00725 if (packet == 0) packet = dmpPacketBuffer; 00726 data[0] = (packet[28] << 8) + packet[29]; 00727 data[1] = (packet[30] << 8) + packet[31]; 00728 data[2] = (packet[32] << 8) + packet[33]; 00729 return 0; 00730 } 00731 // uint8_t MPU9150::dmpSetLinearAccelFilterCoefficient(float coef); 00732 // uint8_t MPU9150::dmpGetLinearAccel(long *data, const uint8_t* packet); 00733 uint8_t MPU9150::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { 00734 // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) 00735 v -> x = vRaw -> x - gravity -> x*4096; 00736 v -> y = vRaw -> y - gravity -> y*4096; 00737 v -> z = vRaw -> z - gravity -> z*4096; 00738 return 0; 00739 } 00740 // uint8_t MPU9150::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); 00741 uint8_t MPU9150::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { 00742 // rotate measured 3D acceleration vector into original state 00743 // frame of reference based on orientation quaternion 00744 memcpy(v, vReal, sizeof(VectorInt16)); 00745 v -> rotate(q); 00746 return 0; 00747 } 00748 // uint8_t MPU9150::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); 00749 // uint8_t MPU9150::dmpGetGyroSensor(long *data, const uint8_t* packet); 00750 // uint8_t MPU9150::dmpGetControlData(long *data, const uint8_t* packet); 00751 // uint8_t MPU9150::dmpGetTemperature(long *data, const uint8_t* packet); 00752 // uint8_t MPU9150::dmpGetGravity(long *data, const uint8_t* packet); 00753 uint8_t MPU9150::dmpGetGravity(VectorFloat *v, Quaternion *q) { 00754 v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); 00755 v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); 00756 v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; 00757 return 0; 00758 } 00759 // uint8_t MPU9150::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); 00760 // uint8_t MPU9150::dmpGetQuantizedAccel(long *data, const uint8_t* packet); 00761 // uint8_t MPU9150::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); 00762 // uint8_t MPU9150::dmpGetEIS(long *data, const uint8_t* packet); 00763 00764 uint8_t MPU9150::dmpGetEuler(float *data, Quaternion *q) { 00765 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 00766 data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta 00767 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 00768 return 0; 00769 } 00770 uint8_t MPU9150::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { 00771 // yaw: (about Z axis) 00772 data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); 00773 // pitch: (nose up/down, about Y axis) 00774 data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); 00775 // roll: (tilt left/right, about X axis) 00776 data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); 00777 return 0; 00778 } 00779 00780 // uint8_t MPU9150::dmpGetAccelFloat(float *data, const uint8_t* packet); 00781 // uint8_t MPU9150::dmpGetQuaternionFloat(float *data, const uint8_t* packet); 00782 00783 uint8_t MPU9150::dmpProcessFIFOPacket(const unsigned char *dmpData) { 00784 /*for (uint8_t k = 0; k < dmpPacketSize; k++) { 00785 if (dmpData[k] < 0x10) Serial.print("0"); 00786 Serial.print(dmpData[k], HEX); 00787 Serial.print(" "); 00788 } 00789 Serial.print("\n");*/ 00790 //Serial.println((uint16_t)dmpPacketBuffer); 00791 return 0; 00792 } 00793 uint8_t MPU9150::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { 00794 uint8_t status; 00795 uint8_t buf[dmpPacketSize]; 00796 for (uint8_t i = 0; i < numPackets; i++) { 00797 // read packet from FIFO 00798 getFIFOBytes(buf, dmpPacketSize); 00799 00800 // process packet 00801 if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; 00802 00803 // increment external process count variable, if supplied 00804 if (processed != 0) *processed++; 00805 } 00806 return 0; 00807 } 00808 00809 // uint8_t MPU9150::dmpSetFIFOProcessedCallback(void (*func) (void)); 00810 00811 // uint8_t MPU9150::dmpInitFIFOParam(); 00812 // uint8_t MPU9150::dmpCloseFIFO(); 00813 // uint8_t MPU9150::dmpSetGyroDataSource(uint_fast8_t source); 00814 // uint8_t MPU9150::dmpDecodeQuantizedAccel(); 00815 // uint32_t MPU9150::dmpGetGyroSumOfSquare(); 00816 // uint32_t MPU9150::dmpGetAccelSumOfSquare(); 00817 // void MPU9150::dmpOverrideQuaternion(long *q); 00818 uint16_t MPU9150::dmpGetFIFOPacketSize() { 00819 return dmpPacketSize; 00820 } 00821 00822 #endif /* _MPU9150_9AXIS_MOTIONAPPS41_H_ */
Generated on Thu Jul 14 2022 00:03:37 by
1.7.2