Johannes Stratmann
Lib for MPU9150/9250 IMU sensors
MPU9150_9Axis_MotionApps41.h@0:ab00d9bcd00d, 2017-04-24 (annotated)
- Committer:
- JojoS
- Date:
- Mon Apr 24 16:21:35 2017 +0000
- Revision:
- 0:ab00d9bcd00d
modified for mbed
Who changed what in which revision?
| User | Revision | Line number | New contents of line |
|---|---|---|---|
| JojoS | 0:ab00d9bcd00d | 1 | // I2Cdev library collection - MPU9150 I2C device class, 9-axis MotionApps 4.1 implementation |
| JojoS | 0:ab00d9bcd00d | 2 | // Based on InvenSense MPU-9150 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) |
| JojoS | 0:ab00d9bcd00d | 3 | // 6/18/2012 by Jeff Rowberg <jeff@rowberg.net> |
| JojoS | 0:ab00d9bcd00d | 4 | // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib |
| JojoS | 0:ab00d9bcd00d | 5 | // |
| JojoS | 0:ab00d9bcd00d | 6 | // Changelog: |
| JojoS | 0:ab00d9bcd00d | 7 | // ... - ongoing debug release |
| JojoS | 0:ab00d9bcd00d | 8 | |
| JojoS | 0:ab00d9bcd00d | 9 | /* ============================================ |
| JojoS | 0:ab00d9bcd00d | 10 | I2Cdev device library code is placed under the MIT license |
| JojoS | 0:ab00d9bcd00d | 11 | Copyright (c) 2012 Jeff Rowberg |
| JojoS | 0:ab00d9bcd00d | 12 | |
| JojoS | 0:ab00d9bcd00d | 13 | Permission is hereby granted, free of charge, to any person obtaining a copy |
| JojoS | 0:ab00d9bcd00d | 14 | of this software and associated documentation files (the "Software"), to deal |
| JojoS | 0:ab00d9bcd00d | 15 | in the Software without restriction, including without limitation the rights |
| JojoS | 0:ab00d9bcd00d | 16 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell |
| JojoS | 0:ab00d9bcd00d | 17 | copies of the Software, and to permit persons to whom the Software is |
| JojoS | 0:ab00d9bcd00d | 18 | furnished to do so, subject to the following conditions: |
| JojoS | 0:ab00d9bcd00d | 19 | |
| JojoS | 0:ab00d9bcd00d | 20 | The above copyright notice and this permission notice shall be included in |
| JojoS | 0:ab00d9bcd00d | 21 | all copies or substantial portions of the Software. |
| JojoS | 0:ab00d9bcd00d | 22 | |
| JojoS | 0:ab00d9bcd00d | 23 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR |
| JojoS | 0:ab00d9bcd00d | 24 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, |
| JojoS | 0:ab00d9bcd00d | 25 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE |
| JojoS | 0:ab00d9bcd00d | 26 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER |
| JojoS | 0:ab00d9bcd00d | 27 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, |
| JojoS | 0:ab00d9bcd00d | 28 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN |
| JojoS | 0:ab00d9bcd00d | 29 | THE SOFTWARE. |
| JojoS | 0:ab00d9bcd00d | 30 | =============================================== |
| JojoS | 0:ab00d9bcd00d | 31 | */ |
| JojoS | 0:ab00d9bcd00d | 32 | |
| JojoS | 0:ab00d9bcd00d | 33 | #ifndef _MPU9150_9AXIS_MOTIONAPPS41_H_ |
| JojoS | 0:ab00d9bcd00d | 34 | #define _MPU9150_9AXIS_MOTIONAPPS41_H_ |
| JojoS | 0:ab00d9bcd00d | 35 | |
| JojoS | 0:ab00d9bcd00d | 36 | #include "I2Cdev.h" |
| JojoS | 0:ab00d9bcd00d | 37 | #include "helper_3dmath.h" |
| JojoS | 0:ab00d9bcd00d | 38 | |
| JojoS | 0:ab00d9bcd00d | 39 | // MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board |
| JojoS | 0:ab00d9bcd00d | 40 | #define MPU9150_INCLUDE_DMP_MOTIONAPPS41 |
| JojoS | 0:ab00d9bcd00d | 41 | |
| JojoS | 0:ab00d9bcd00d | 42 | #include "MPU9150.h" |
| JojoS | 0:ab00d9bcd00d | 43 | |
| JojoS | 0:ab00d9bcd00d | 44 | // Tom Carpenter's conditional PROGMEM code |
| JojoS | 0:ab00d9bcd00d | 45 | // http://forum.arduino.cc/index.php?topic=129407.0 |
| JojoS | 0:ab00d9bcd00d | 46 | #ifndef __arm__ |
| JojoS | 0:ab00d9bcd00d | 47 | #include <avr/pgmspace.h> |
| JojoS | 0:ab00d9bcd00d | 48 | #else |
| JojoS | 0:ab00d9bcd00d | 49 | // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen |
| JojoS | 0:ab00d9bcd00d | 50 | #ifndef __PGMSPACE_H_ |
| JojoS | 0:ab00d9bcd00d | 51 | #define __PGMSPACE_H_ 1 |
| JojoS | 0:ab00d9bcd00d | 52 | #include <inttypes.h> |
| JojoS | 0:ab00d9bcd00d | 53 | |
| JojoS | 0:ab00d9bcd00d | 54 | #define PROGMEM |
| JojoS | 0:ab00d9bcd00d | 55 | #define PGM_P const char * |
| JojoS | 0:ab00d9bcd00d | 56 | #define PSTR(str) (str) |
| JojoS | 0:ab00d9bcd00d | 57 | #define F(x) x |
| JojoS | 0:ab00d9bcd00d | 58 | |
| JojoS | 0:ab00d9bcd00d | 59 | typedef void prog_void; |
| JojoS | 0:ab00d9bcd00d | 60 | typedef char prog_char; |
| JojoS | 0:ab00d9bcd00d | 61 | typedef unsigned char prog_uchar; |
| JojoS | 0:ab00d9bcd00d | 62 | typedef int8_t prog_int8_t; |
| JojoS | 0:ab00d9bcd00d | 63 | typedef uint8_t prog_uint8_t; |
| JojoS | 0:ab00d9bcd00d | 64 | typedef int16_t prog_int16_t; |
| JojoS | 0:ab00d9bcd00d | 65 | typedef uint16_t prog_uint16_t; |
| JojoS | 0:ab00d9bcd00d | 66 | typedef int32_t prog_int32_t; |
| JojoS | 0:ab00d9bcd00d | 67 | typedef uint32_t prog_uint32_t; |
| JojoS | 0:ab00d9bcd00d | 68 | |
| JojoS | 0:ab00d9bcd00d | 69 | #define strcpy_P(dest, src) strcpy((dest), (src)) |
| JojoS | 0:ab00d9bcd00d | 70 | #define strcat_P(dest, src) strcat((dest), (src)) |
| JojoS | 0:ab00d9bcd00d | 71 | #define strcmp_P(a, b) strcmp((a), (b)) |
| JojoS | 0:ab00d9bcd00d | 72 | |
| JojoS | 0:ab00d9bcd00d | 73 | #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) |
| JojoS | 0:ab00d9bcd00d | 74 | #define pgm_read_word(addr) (*(const unsigned short *)(addr)) |
| JojoS | 0:ab00d9bcd00d | 75 | #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) |
| JojoS | 0:ab00d9bcd00d | 76 | #define pgm_read_float(addr) (*(const float *)(addr)) |
| JojoS | 0:ab00d9bcd00d | 77 | |
| JojoS | 0:ab00d9bcd00d | 78 | #define pgm_read_byte_near(addr) pgm_read_byte(addr) |
| JojoS | 0:ab00d9bcd00d | 79 | #define pgm_read_word_near(addr) pgm_read_word(addr) |
| JojoS | 0:ab00d9bcd00d | 80 | #define pgm_read_dword_near(addr) pgm_read_dword(addr) |
| JojoS | 0:ab00d9bcd00d | 81 | #define pgm_read_float_near(addr) pgm_read_float(addr) |
| JojoS | 0:ab00d9bcd00d | 82 | #define pgm_read_byte_far(addr) pgm_read_byte(addr) |
| JojoS | 0:ab00d9bcd00d | 83 | #define pgm_read_word_far(addr) pgm_read_word(addr) |
| JojoS | 0:ab00d9bcd00d | 84 | #define pgm_read_dword_far(addr) pgm_read_dword(addr) |
| JojoS | 0:ab00d9bcd00d | 85 | #define pgm_read_float_far(addr) pgm_read_float(addr) |
| JojoS | 0:ab00d9bcd00d | 86 | #endif |
| JojoS | 0:ab00d9bcd00d | 87 | #endif |
| JojoS | 0:ab00d9bcd00d | 88 | |
| JojoS | 0:ab00d9bcd00d | 89 | // NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. |
| JojoS | 0:ab00d9bcd00d | 90 | // Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) |
| JojoS | 0:ab00d9bcd00d | 91 | // after moving string constants to flash memory storage using the F() |
| JojoS | 0:ab00d9bcd00d | 92 | // compiler macro (Arduino IDE 1.0+ required). |
| JojoS | 0:ab00d9bcd00d | 93 | |
| JojoS | 0:ab00d9bcd00d | 94 | //#define DEBUG |
| JojoS | 0:ab00d9bcd00d | 95 | #ifdef DEBUG |
| JojoS | 0:ab00d9bcd00d | 96 | #define DEBUG_PRINT(x) Serial.print(x) |
| JojoS | 0:ab00d9bcd00d | 97 | #define DEBUG_PRINTF(x, y) Serial.print(x, y) |
| JojoS | 0:ab00d9bcd00d | 98 | #define DEBUG_PRINTLN(x) Serial.println(x) |
| JojoS | 0:ab00d9bcd00d | 99 | #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) |
| JojoS | 0:ab00d9bcd00d | 100 | #else |
| JojoS | 0:ab00d9bcd00d | 101 | #define DEBUG_PRINT(x) |
| JojoS | 0:ab00d9bcd00d | 102 | #define DEBUG_PRINTF(x, y) |
| JojoS | 0:ab00d9bcd00d | 103 | #define DEBUG_PRINTLN(x) |
| JojoS | 0:ab00d9bcd00d | 104 | #define DEBUG_PRINTLNF(x, y) |
| JojoS | 0:ab00d9bcd00d | 105 | #endif |
| JojoS | 0:ab00d9bcd00d | 106 | |
| JojoS | 0:ab00d9bcd00d | 107 | #define MPU9150_DMP_CODE_SIZE 1962 // dmpMemory[] |
| JojoS | 0:ab00d9bcd00d | 108 | #define MPU9150_DMP_CONFIG_SIZE 232 // dmpConfig[] |
| JojoS | 0:ab00d9bcd00d | 109 | #define MPU9150_DMP_UPDATES_SIZE 140 // dmpUpdates[] |
| JojoS | 0:ab00d9bcd00d | 110 | |
| JojoS | 0:ab00d9bcd00d | 111 | /* ================================================================================================ * |
| JojoS | 0:ab00d9bcd00d | 112 | | Default MotionApps v4.1 48-byte FIFO packet structure: | |
| JojoS | 0:ab00d9bcd00d | 113 | | | |
| JojoS | 0:ab00d9bcd00d | 114 | | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | |
| JojoS | 0:ab00d9bcd00d | 115 | | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | |
| JojoS | 0:ab00d9bcd00d | 116 | | | |
| JojoS | 0:ab00d9bcd00d | 117 | | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | |
| JojoS | 0:ab00d9bcd00d | 118 | | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | |
| JojoS | 0:ab00d9bcd00d | 119 | * ================================================================================================ */ |
| JojoS | 0:ab00d9bcd00d | 120 | |
| JojoS | 0:ab00d9bcd00d | 121 | // this block of memory gets written to the MPU on start-up, and it seems |
| JojoS | 0:ab00d9bcd00d | 122 | // to be volatile memory, so it has to be done each time (it only takes ~1 |
| JojoS | 0:ab00d9bcd00d | 123 | // second though) |
| JojoS | 0:ab00d9bcd00d | 124 | const prog_uchar dmpMemory[MPU9150_DMP_CODE_SIZE] PROGMEM = { |
| JojoS | 0:ab00d9bcd00d | 125 | // bank 0, 256 bytes |
| JojoS | 0:ab00d9bcd00d | 126 | 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 127 | 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, |
| JojoS | 0:ab00d9bcd00d | 128 | 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 129 | 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 130 | 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, |
| JojoS | 0:ab00d9bcd00d | 131 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 132 | 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 133 | 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 134 | 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, |
| JojoS | 0:ab00d9bcd00d | 135 | 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, |
| JojoS | 0:ab00d9bcd00d | 136 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 137 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, |
| JojoS | 0:ab00d9bcd00d | 138 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 139 | 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, |
| JojoS | 0:ab00d9bcd00d | 140 | 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, |
| JojoS | 0:ab00d9bcd00d | 141 | 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, |
| JojoS | 0:ab00d9bcd00d | 142 | |
| JojoS | 0:ab00d9bcd00d | 143 | // bank 1, 256 bytes |
| JojoS | 0:ab00d9bcd00d | 144 | 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 145 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 146 | 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, |
| JojoS | 0:ab00d9bcd00d | 147 | 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, |
| JojoS | 0:ab00d9bcd00d | 148 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, |
| JojoS | 0:ab00d9bcd00d | 149 | 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 150 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, |
| JojoS | 0:ab00d9bcd00d | 151 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 152 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 153 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 154 | 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, |
| JojoS | 0:ab00d9bcd00d | 155 | 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 156 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, |
| JojoS | 0:ab00d9bcd00d | 157 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 158 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 159 | 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, |
| JojoS | 0:ab00d9bcd00d | 160 | |
| JojoS | 0:ab00d9bcd00d | 161 | // bank 2, 256 bytes |
| JojoS | 0:ab00d9bcd00d | 162 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 163 | 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, |
| JojoS | 0:ab00d9bcd00d | 164 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 165 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 166 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 167 | 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 168 | 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 169 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 170 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 171 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 172 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 173 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 174 | 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 175 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 176 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, |
| JojoS | 0:ab00d9bcd00d | 177 | 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 178 | |
| JojoS | 0:ab00d9bcd00d | 179 | // bank 3, 256 bytes |
| JojoS | 0:ab00d9bcd00d | 180 | 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, |
| JojoS | 0:ab00d9bcd00d | 181 | 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, |
| JojoS | 0:ab00d9bcd00d | 182 | 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, |
| JojoS | 0:ab00d9bcd00d | 183 | 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, |
| JojoS | 0:ab00d9bcd00d | 184 | 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, |
| JojoS | 0:ab00d9bcd00d | 185 | 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, |
| JojoS | 0:ab00d9bcd00d | 186 | 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, |
| JojoS | 0:ab00d9bcd00d | 187 | 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, |
| JojoS | 0:ab00d9bcd00d | 188 | 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, |
| JojoS | 0:ab00d9bcd00d | 189 | 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, |
| JojoS | 0:ab00d9bcd00d | 190 | 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, |
| JojoS | 0:ab00d9bcd00d | 191 | 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, |
| JojoS | 0:ab00d9bcd00d | 192 | 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, |
| JojoS | 0:ab00d9bcd00d | 193 | 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, |
| JojoS | 0:ab00d9bcd00d | 194 | 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, |
| JojoS | 0:ab00d9bcd00d | 195 | 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, |
| JojoS | 0:ab00d9bcd00d | 196 | |
| JojoS | 0:ab00d9bcd00d | 197 | // bank 4, 256 bytes |
| JojoS | 0:ab00d9bcd00d | 198 | 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, |
| JojoS | 0:ab00d9bcd00d | 199 | 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, |
| JojoS | 0:ab00d9bcd00d | 200 | 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, |
| JojoS | 0:ab00d9bcd00d | 201 | 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, |
| JojoS | 0:ab00d9bcd00d | 202 | 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, |
| JojoS | 0:ab00d9bcd00d | 203 | 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, |
| JojoS | 0:ab00d9bcd00d | 204 | 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, |
| JojoS | 0:ab00d9bcd00d | 205 | 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, |
| JojoS | 0:ab00d9bcd00d | 206 | 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, |
| JojoS | 0:ab00d9bcd00d | 207 | 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, |
| JojoS | 0:ab00d9bcd00d | 208 | 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, |
| JojoS | 0:ab00d9bcd00d | 209 | 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, |
| JojoS | 0:ab00d9bcd00d | 210 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
| JojoS | 0:ab00d9bcd00d | 211 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, |
| JojoS | 0:ab00d9bcd00d | 212 | 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, |
| JojoS | 0:ab00d9bcd00d | 213 | 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, |
| JojoS | 0:ab00d9bcd00d | 214 | |
| JojoS | 0:ab00d9bcd00d | 215 | // bank 5, 256 bytes |
| JojoS | 0:ab00d9bcd00d | 216 | 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, |
| JojoS | 0:ab00d9bcd00d | 217 | 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, |
| JojoS | 0:ab00d9bcd00d | 218 | 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, |
| JojoS | 0:ab00d9bcd00d | 219 | 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, |
| JojoS | 0:ab00d9bcd00d | 220 | 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, |
| JojoS | 0:ab00d9bcd00d | 221 | 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, |
| JojoS | 0:ab00d9bcd00d | 222 | 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, |
| JojoS | 0:ab00d9bcd00d | 223 | 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, |
| JojoS | 0:ab00d9bcd00d | 224 | 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, |
| JojoS | 0:ab00d9bcd00d | 225 | 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, |
| JojoS | 0:ab00d9bcd00d | 226 | 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, |
| JojoS | 0:ab00d9bcd00d | 227 | 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, |
| JojoS | 0:ab00d9bcd00d | 228 | 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, |
| JojoS | 0:ab00d9bcd00d | 229 | 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, |
| JojoS | 0:ab00d9bcd00d | 230 | 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, |
| JojoS | 0:ab00d9bcd00d | 231 | 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, |
| JojoS | 0:ab00d9bcd00d | 232 | |
| JojoS | 0:ab00d9bcd00d | 233 | // bank 6, 256 bytes |
| JojoS | 0:ab00d9bcd00d | 234 | 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, |
| JojoS | 0:ab00d9bcd00d | 235 | 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, |
| JojoS | 0:ab00d9bcd00d | 236 | 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, |
| JojoS | 0:ab00d9bcd00d | 237 | 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, |
| JojoS | 0:ab00d9bcd00d | 238 | 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, |
| JojoS | 0:ab00d9bcd00d | 239 | 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, |
| JojoS | 0:ab00d9bcd00d | 240 | 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, |
| JojoS | 0:ab00d9bcd00d | 241 | 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, |
| JojoS | 0:ab00d9bcd00d | 242 | 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, |
| JojoS | 0:ab00d9bcd00d | 243 | 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, |
| JojoS | 0:ab00d9bcd00d | 244 | 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, |
| JojoS | 0:ab00d9bcd00d | 245 | 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, |
| JojoS | 0:ab00d9bcd00d | 246 | 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, |
| JojoS | 0:ab00d9bcd00d | 247 | 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, |
| JojoS | 0:ab00d9bcd00d | 248 | 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, |
| JojoS | 0:ab00d9bcd00d | 249 | 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, |
| JojoS | 0:ab00d9bcd00d | 250 | |
| JojoS | 0:ab00d9bcd00d | 251 | // bank 7, 170 bytes (remainder) |
| JojoS | 0:ab00d9bcd00d | 252 | 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, |
| JojoS | 0:ab00d9bcd00d | 253 | 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, |
| JojoS | 0:ab00d9bcd00d | 254 | 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, |
| JojoS | 0:ab00d9bcd00d | 255 | 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, |
| JojoS | 0:ab00d9bcd00d | 256 | 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, |
| JojoS | 0:ab00d9bcd00d | 257 | 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, |
| JojoS | 0:ab00d9bcd00d | 258 | 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, |
| JojoS | 0:ab00d9bcd00d | 259 | 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, |
| JojoS | 0:ab00d9bcd00d | 260 | 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, |
| JojoS | 0:ab00d9bcd00d | 261 | 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, |
| JojoS | 0:ab00d9bcd00d | 262 | 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF |
| JojoS | 0:ab00d9bcd00d | 263 | }; |
| JojoS | 0:ab00d9bcd00d | 264 | |
| JojoS | 0:ab00d9bcd00d | 265 | const prog_uchar dmpConfig[MPU9150_DMP_CONFIG_SIZE] PROGMEM = { |
| JojoS | 0:ab00d9bcd00d | 266 | // BANK OFFSET LENGTH [DATA] |
| JojoS | 0:ab00d9bcd00d | 267 | 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? |
| JojoS | 0:ab00d9bcd00d | 268 | 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration |
| JojoS | 0:ab00d9bcd00d | 269 | 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration |
| JojoS | 0:ab00d9bcd00d | 270 | 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration |
| JojoS | 0:ab00d9bcd00d | 271 | 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration |
| JojoS | 0:ab00d9bcd00d | 272 | 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration |
| JojoS | 0:ab00d9bcd00d | 273 | 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration |
| JojoS | 0:ab00d9bcd00d | 274 | 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration |
| JojoS | 0:ab00d9bcd00d | 275 | |
| JojoS | 0:ab00d9bcd00d | 276 | 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration |
| JojoS | 0:ab00d9bcd00d | 277 | 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 |
| JojoS | 0:ab00d9bcd00d | 278 | 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 |
| JojoS | 0:ab00d9bcd00d | 279 | 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 |
| JojoS | 0:ab00d9bcd00d | 280 | 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 |
| JojoS | 0:ab00d9bcd00d | 281 | 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 |
| JojoS | 0:ab00d9bcd00d | 282 | 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 |
| JojoS | 0:ab00d9bcd00d | 283 | 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 |
| JojoS | 0:ab00d9bcd00d | 284 | 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 |
| JojoS | 0:ab00d9bcd00d | 285 | |
| JojoS | 0:ab00d9bcd00d | 286 | 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel |
| JojoS | 0:ab00d9bcd00d | 287 | 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors |
| JojoS | 0:ab00d9bcd00d | 288 | 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion |
| JojoS | 0:ab00d9bcd00d | 289 | 0x00, 0xA3, 0x01, 0x00, // ? |
| JojoS | 0:ab00d9bcd00d | 290 | 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update |
| JojoS | 0:ab00d9bcd00d | 291 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion |
| JojoS | 0:ab00d9bcd00d | 292 | 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer |
| JojoS | 0:ab00d9bcd00d | 293 | 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro |
| JojoS | 0:ab00d9bcd00d | 294 | 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo |
| JojoS | 0:ab00d9bcd00d | 295 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? |
| JojoS | 0:ab00d9bcd00d | 296 | 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? |
| JojoS | 0:ab00d9bcd00d | 297 | 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? |
| JojoS | 0:ab00d9bcd00d | 298 | // SPECIAL 0x01 = enable interrupts |
| JojoS | 0:ab00d9bcd00d | 299 | 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION |
| JojoS | 0:ab00d9bcd00d | 300 | 0x07, 0xA7, 0x01, 0xFE, // ? |
| JojoS | 0:ab00d9bcd00d | 301 | 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? |
| JojoS | 0:ab00d9bcd00d | 302 | 0x07, 0x67, 0x01, 0x9A, // ? |
| JojoS | 0:ab00d9bcd00d | 303 | 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo |
| JojoS | 0:ab00d9bcd00d | 304 | 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo |
| JojoS | 0:ab00d9bcd00d | 305 | 0x02, 0x16, 0x02, 0x00, 0x03 // D_0_22 inv_set_fifo_rate |
| JojoS | 0:ab00d9bcd00d | 306 | |
| JojoS | 0:ab00d9bcd00d | 307 | // This very last 0x01 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, |
| JojoS | 0:ab00d9bcd00d | 308 | // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. |
| JojoS | 0:ab00d9bcd00d | 309 | // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) |
| JojoS | 0:ab00d9bcd00d | 310 | |
| JojoS | 0:ab00d9bcd00d | 311 | // It is important to make sure the host processor can keep up with reading and processing |
| JojoS | 0:ab00d9bcd00d | 312 | // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. |
| JojoS | 0:ab00d9bcd00d | 313 | }; |
| JojoS | 0:ab00d9bcd00d | 314 | |
| JojoS | 0:ab00d9bcd00d | 315 | const prog_uchar dmpUpdates[MPU9150_DMP_UPDATES_SIZE] PROGMEM = { |
| JojoS | 0:ab00d9bcd00d | 316 | 0x01, 0xB2, 0x02, 0xFF, 0xF5, |
| JojoS | 0:ab00d9bcd00d | 317 | 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, |
| JojoS | 0:ab00d9bcd00d | 318 | 0x00, 0xA3, 0x01, 0x00, |
| JojoS | 0:ab00d9bcd00d | 319 | 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, |
| JojoS | 0:ab00d9bcd00d | 320 | 0x01, 0x6A, 0x02, 0x06, 0x00, |
| JojoS | 0:ab00d9bcd00d | 321 | 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 322 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 323 | 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 324 | 0x01, 0x08, 0x02, 0x01, 0x20, |
| JojoS | 0:ab00d9bcd00d | 325 | 0x01, 0x0A, 0x02, 0x00, 0x4E, |
| JojoS | 0:ab00d9bcd00d | 326 | 0x01, 0x02, 0x02, 0xFE, 0xB3, |
| JojoS | 0:ab00d9bcd00d | 327 | 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ |
| JojoS | 0:ab00d9bcd00d | 328 | 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 329 | 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, |
| JojoS | 0:ab00d9bcd00d | 330 | 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 331 | 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 332 | 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 333 | 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, |
| JojoS | 0:ab00d9bcd00d | 334 | 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 |
| JojoS | 0:ab00d9bcd00d | 335 | }; |
| JojoS | 0:ab00d9bcd00d | 336 | |
| JojoS | 0:ab00d9bcd00d | 337 | uint8_t MPU9150::dmpInitialize() { |
| JojoS | 0:ab00d9bcd00d | 338 | // reset device |
| JojoS | 0:ab00d9bcd00d | 339 | DEBUG_PRINTLN(F("\n\nResetting MPU9150...")); |
| JojoS | 0:ab00d9bcd00d | 340 | reset(); |
| JojoS | 0:ab00d9bcd00d | 341 | delay(30); // wait after reset |
| JojoS | 0:ab00d9bcd00d | 342 | |
| JojoS | 0:ab00d9bcd00d | 343 | // disable sleep mode |
| JojoS | 0:ab00d9bcd00d | 344 | DEBUG_PRINTLN(F("Disabling sleep mode...")); |
| JojoS | 0:ab00d9bcd00d | 345 | setSleepEnabled(false); |
| JojoS | 0:ab00d9bcd00d | 346 | |
| JojoS | 0:ab00d9bcd00d | 347 | // get MPU product ID |
| JojoS | 0:ab00d9bcd00d | 348 | DEBUG_PRINTLN(F("Getting product ID...")); |
| JojoS | 0:ab00d9bcd00d | 349 | //uint8_t productID = 0; //getProductID(); |
| JojoS | 0:ab00d9bcd00d | 350 | DEBUG_PRINT(F("Product ID = ")); |
| JojoS | 0:ab00d9bcd00d | 351 | DEBUG_PRINT(productID); |
| JojoS | 0:ab00d9bcd00d | 352 | |
| JojoS | 0:ab00d9bcd00d | 353 | // get MPU hardware revision |
| JojoS | 0:ab00d9bcd00d | 354 | DEBUG_PRINTLN(F("Selecting user bank 16...")); |
| JojoS | 0:ab00d9bcd00d | 355 | setMemoryBank(0x10, true, true); |
| JojoS | 0:ab00d9bcd00d | 356 | DEBUG_PRINTLN(F("Selecting memory byte 6...")); |
| JojoS | 0:ab00d9bcd00d | 357 | setMemoryStartAddress(0x06); |
| JojoS | 0:ab00d9bcd00d | 358 | DEBUG_PRINTLN(F("Checking hardware revision...")); |
| JojoS | 0:ab00d9bcd00d | 359 | uint8_t hwRevision = readMemoryByte(); |
| JojoS | 0:ab00d9bcd00d | 360 | DEBUG_PRINT(F("Revision @ user[16][6] = ")); |
| JojoS | 0:ab00d9bcd00d | 361 | DEBUG_PRINTLNF(hwRevision, HEX); |
| JojoS | 0:ab00d9bcd00d | 362 | DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); |
| JojoS | 0:ab00d9bcd00d | 363 | setMemoryBank(0, false, false); |
| JojoS | 0:ab00d9bcd00d | 364 | |
| JojoS | 0:ab00d9bcd00d | 365 | // check OTP bank valid |
| JojoS | 0:ab00d9bcd00d | 366 | DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); |
| JojoS | 0:ab00d9bcd00d | 367 | uint8_t otpValid = getOTPBankValid(); |
| JojoS | 0:ab00d9bcd00d | 368 | DEBUG_PRINT(F("OTP bank is ")); |
| JojoS | 0:ab00d9bcd00d | 369 | DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); |
| JojoS | 0:ab00d9bcd00d | 370 | |
| JojoS | 0:ab00d9bcd00d | 371 | // get X/Y/Z gyro offsets |
| JojoS | 0:ab00d9bcd00d | 372 | DEBUG_PRINTLN(F("Reading gyro offset values...")); |
| JojoS | 0:ab00d9bcd00d | 373 | int8_t xgOffset = getXGyroOffset(); |
| JojoS | 0:ab00d9bcd00d | 374 | int8_t ygOffset = getYGyroOffset(); |
| JojoS | 0:ab00d9bcd00d | 375 | int8_t zgOffset = getZGyroOffset(); |
| JojoS | 0:ab00d9bcd00d | 376 | DEBUG_PRINT(F("X gyro offset = ")); |
| JojoS | 0:ab00d9bcd00d | 377 | DEBUG_PRINTLN(xgOffset); |
| JojoS | 0:ab00d9bcd00d | 378 | DEBUG_PRINT(F("Y gyro offset = ")); |
| JojoS | 0:ab00d9bcd00d | 379 | DEBUG_PRINTLN(ygOffset); |
| JojoS | 0:ab00d9bcd00d | 380 | DEBUG_PRINT(F("Z gyro offset = ")); |
| JojoS | 0:ab00d9bcd00d | 381 | DEBUG_PRINTLN(zgOffset); |
| JojoS | 0:ab00d9bcd00d | 382 | |
| JojoS | 0:ab00d9bcd00d | 383 | I2Cdev::readByte(devAddr, MPU9150_RA_USER_CTRL, buffer); // ? |
| JojoS | 0:ab00d9bcd00d | 384 | |
| JojoS | 0:ab00d9bcd00d | 385 | DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); |
| JojoS | 0:ab00d9bcd00d | 386 | I2Cdev::writeByte(devAddr, MPU9150_RA_INT_PIN_CFG, 0x32); |
| JojoS | 0:ab00d9bcd00d | 387 | |
| JojoS | 0:ab00d9bcd00d | 388 | // enable MPU AUX I2C bypass mode |
| JojoS | 0:ab00d9bcd00d | 389 | //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); |
| JojoS | 0:ab00d9bcd00d | 390 | //setI2CBypassEnabled(true); |
| JojoS | 0:ab00d9bcd00d | 391 | |
| JojoS | 0:ab00d9bcd00d | 392 | DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); |
| JojoS | 0:ab00d9bcd00d | 393 | //mag -> setMode(0); |
| JojoS | 0:ab00d9bcd00d | 394 | I2Cdev::writeByte(0x0E, 0x0A, 0x00); |
| JojoS | 0:ab00d9bcd00d | 395 | |
| JojoS | 0:ab00d9bcd00d | 396 | DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); |
| JojoS | 0:ab00d9bcd00d | 397 | //mag -> setMode(0x0F); |
| JojoS | 0:ab00d9bcd00d | 398 | I2Cdev::writeByte(0x0E, 0x0A, 0x0F); |
| JojoS | 0:ab00d9bcd00d | 399 | |
| JojoS | 0:ab00d9bcd00d | 400 | DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); |
| JojoS | 0:ab00d9bcd00d | 401 | int8_t asax, asay, asaz; |
| JojoS | 0:ab00d9bcd00d | 402 | //mag -> getAdjustment(&asax, &asay, &asaz); |
| JojoS | 0:ab00d9bcd00d | 403 | I2Cdev::readBytes(0x0E, 0x10, 3, buffer); |
| JojoS | 0:ab00d9bcd00d | 404 | asax = (int8_t)buffer[0]; |
| JojoS | 0:ab00d9bcd00d | 405 | asay = (int8_t)buffer[1]; |
| JojoS | 0:ab00d9bcd00d | 406 | asaz = (int8_t)buffer[2]; |
| JojoS | 0:ab00d9bcd00d | 407 | DEBUG_PRINT(F("Adjustment X/Y/Z = ")); |
| JojoS | 0:ab00d9bcd00d | 408 | DEBUG_PRINT(asax); |
| JojoS | 0:ab00d9bcd00d | 409 | DEBUG_PRINT(F(" / ")); |
| JojoS | 0:ab00d9bcd00d | 410 | DEBUG_PRINT(asay); |
| JojoS | 0:ab00d9bcd00d | 411 | DEBUG_PRINT(F(" / ")); |
| JojoS | 0:ab00d9bcd00d | 412 | DEBUG_PRINTLN(asaz); |
| JojoS | 0:ab00d9bcd00d | 413 | |
| JojoS | 0:ab00d9bcd00d | 414 | DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); |
| JojoS | 0:ab00d9bcd00d | 415 | //mag -> setMode(0); |
| JojoS | 0:ab00d9bcd00d | 416 | I2Cdev::writeByte(0x0E, 0x0A, 0x00); |
| JojoS | 0:ab00d9bcd00d | 417 | |
| JojoS | 0:ab00d9bcd00d | 418 | // load DMP code into memory banks |
| JojoS | 0:ab00d9bcd00d | 419 | DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); |
| JojoS | 0:ab00d9bcd00d | 420 | DEBUG_PRINT(MPU9150_DMP_CODE_SIZE); |
| JojoS | 0:ab00d9bcd00d | 421 | DEBUG_PRINTLN(F(" bytes)")); |
| JojoS | 0:ab00d9bcd00d | 422 | if (writeProgMemoryBlock(dmpMemory, MPU9150_DMP_CODE_SIZE)) { |
| JojoS | 0:ab00d9bcd00d | 423 | DEBUG_PRINTLN(F("Success! DMP code written and verified.")); |
| JojoS | 0:ab00d9bcd00d | 424 | |
| JojoS | 0:ab00d9bcd00d | 425 | DEBUG_PRINTLN(F("Configuring DMP and related settings...")); |
| JojoS | 0:ab00d9bcd00d | 426 | |
| JojoS | 0:ab00d9bcd00d | 427 | // write DMP configuration |
| JojoS | 0:ab00d9bcd00d | 428 | DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); |
| JojoS | 0:ab00d9bcd00d | 429 | DEBUG_PRINT(MPU9150_DMP_CONFIG_SIZE); |
| JojoS | 0:ab00d9bcd00d | 430 | DEBUG_PRINTLN(F(" bytes in config def)")); |
| JojoS | 0:ab00d9bcd00d | 431 | if (writeProgDMPConfigurationSet(dmpConfig, MPU9150_DMP_CONFIG_SIZE)) { |
| JojoS | 0:ab00d9bcd00d | 432 | DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); |
| JojoS | 0:ab00d9bcd00d | 433 | |
| JojoS | 0:ab00d9bcd00d | 434 | DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); |
| JojoS | 0:ab00d9bcd00d | 435 | setIntEnabled(0x12); |
| JojoS | 0:ab00d9bcd00d | 436 | |
| JojoS | 0:ab00d9bcd00d | 437 | DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); |
| JojoS | 0:ab00d9bcd00d | 438 | setRate(4); // 1khz / (1 + 4) = 200 Hz |
| JojoS | 0:ab00d9bcd00d | 439 | |
| JojoS | 0:ab00d9bcd00d | 440 | DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); |
| JojoS | 0:ab00d9bcd00d | 441 | setClockSource(MPU9150_CLOCK_PLL_ZGYRO); |
| JojoS | 0:ab00d9bcd00d | 442 | |
| JojoS | 0:ab00d9bcd00d | 443 | DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); |
| JojoS | 0:ab00d9bcd00d | 444 | setDLPFMode(MPU9150_DLPF_BW_42); |
| JojoS | 0:ab00d9bcd00d | 445 | |
| JojoS | 0:ab00d9bcd00d | 446 | DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); |
| JojoS | 0:ab00d9bcd00d | 447 | setExternalFrameSync(MPU9150_EXT_SYNC_TEMP_OUT_L); |
| JojoS | 0:ab00d9bcd00d | 448 | |
| JojoS | 0:ab00d9bcd00d | 449 | DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); |
| JojoS | 0:ab00d9bcd00d | 450 | setFullScaleGyroRange(MPU9150_GYRO_FS_2000); |
| JojoS | 0:ab00d9bcd00d | 451 | |
| JojoS | 0:ab00d9bcd00d | 452 | DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 453 | setDMPConfig1(0x03); |
| JojoS | 0:ab00d9bcd00d | 454 | setDMPConfig2(0x00); |
| JojoS | 0:ab00d9bcd00d | 455 | |
| JojoS | 0:ab00d9bcd00d | 456 | DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); |
| JojoS | 0:ab00d9bcd00d | 457 | setOTPBankValid(false); |
| JojoS | 0:ab00d9bcd00d | 458 | |
| JojoS | 0:ab00d9bcd00d | 459 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values...")); |
| JojoS | 0:ab00d9bcd00d | 460 | setXGyroOffset(xgOffset); |
| JojoS | 0:ab00d9bcd00d | 461 | setYGyroOffset(ygOffset); |
| JojoS | 0:ab00d9bcd00d | 462 | setZGyroOffset(zgOffset); |
| JojoS | 0:ab00d9bcd00d | 463 | |
| JojoS | 0:ab00d9bcd00d | 464 | DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); |
| JojoS | 0:ab00d9bcd00d | 465 | setXGyroOffsetUser(0); |
| JojoS | 0:ab00d9bcd00d | 466 | setYGyroOffsetUser(0); |
| JojoS | 0:ab00d9bcd00d | 467 | setZGyroOffsetUser(0); |
| JojoS | 0:ab00d9bcd00d | 468 | |
| JojoS | 0:ab00d9bcd00d | 469 | DEBUG_PRINTLN(F("Writing final memory update 1/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 470 | uint8_t dmpUpdate[16], j; |
| JojoS | 0:ab00d9bcd00d | 471 | uint16_t pos = 0; |
| JojoS | 0:ab00d9bcd00d | 472 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 473 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 474 | |
| JojoS | 0:ab00d9bcd00d | 475 | DEBUG_PRINTLN(F("Writing final memory update 2/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 476 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 477 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 478 | |
| JojoS | 0:ab00d9bcd00d | 479 | DEBUG_PRINTLN(F("Resetting FIFO...")); |
| JojoS | 0:ab00d9bcd00d | 480 | resetFIFO(); |
| JojoS | 0:ab00d9bcd00d | 481 | |
| JojoS | 0:ab00d9bcd00d | 482 | DEBUG_PRINTLN(F("Reading FIFO count...")); |
| JojoS | 0:ab00d9bcd00d | 483 | uint8_t fifoCount = getFIFOCount(); |
| JojoS | 0:ab00d9bcd00d | 484 | |
| JojoS | 0:ab00d9bcd00d | 485 | DEBUG_PRINT(F("Current FIFO count=")); |
| JojoS | 0:ab00d9bcd00d | 486 | DEBUG_PRINTLN(fifoCount); |
| JojoS | 0:ab00d9bcd00d | 487 | uint8_t fifoBuffer[128]; |
| JojoS | 0:ab00d9bcd00d | 488 | //getFIFOBytes(fifoBuffer, fifoCount); |
| JojoS | 0:ab00d9bcd00d | 489 | |
| JojoS | 0:ab00d9bcd00d | 490 | DEBUG_PRINTLN(F("Writing final memory update 3/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 491 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 492 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 493 | |
| JojoS | 0:ab00d9bcd00d | 494 | DEBUG_PRINTLN(F("Writing final memory update 4/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 495 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 496 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 497 | |
| JojoS | 0:ab00d9bcd00d | 498 | DEBUG_PRINTLN(F("Disabling all standby flags...")); |
| JojoS | 0:ab00d9bcd00d | 499 | I2Cdev::writeByte(0x68, MPU9150_RA_PWR_MGMT_2, 0x00); |
| JojoS | 0:ab00d9bcd00d | 500 | |
| JojoS | 0:ab00d9bcd00d | 501 | DEBUG_PRINTLN(F("Setting accelerometer sensitivity to +/- 2g...")); |
| JojoS | 0:ab00d9bcd00d | 502 | I2Cdev::writeByte(0x68, MPU9150_RA_ACCEL_CONFIG, 0x00); |
| JojoS | 0:ab00d9bcd00d | 503 | |
| JojoS | 0:ab00d9bcd00d | 504 | DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); |
| JojoS | 0:ab00d9bcd00d | 505 | setMotionDetectionThreshold(2); |
| JojoS | 0:ab00d9bcd00d | 506 | |
| JojoS | 0:ab00d9bcd00d | 507 | DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); |
| JojoS | 0:ab00d9bcd00d | 508 | setZeroMotionDetectionThreshold(156); |
| JojoS | 0:ab00d9bcd00d | 509 | |
| JojoS | 0:ab00d9bcd00d | 510 | DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); |
| JojoS | 0:ab00d9bcd00d | 511 | setMotionDetectionDuration(80); |
| JojoS | 0:ab00d9bcd00d | 512 | |
| JojoS | 0:ab00d9bcd00d | 513 | DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); |
| JojoS | 0:ab00d9bcd00d | 514 | setZeroMotionDetectionDuration(0); |
| JojoS | 0:ab00d9bcd00d | 515 | |
| JojoS | 0:ab00d9bcd00d | 516 | DEBUG_PRINTLN(F("Setting AK8975 to single measurement mode...")); |
| JojoS | 0:ab00d9bcd00d | 517 | //mag -> setMode(1); |
| JojoS | 0:ab00d9bcd00d | 518 | I2Cdev::writeByte(0x0E, 0x0A, 0x01); |
| JojoS | 0:ab00d9bcd00d | 519 | |
| JojoS | 0:ab00d9bcd00d | 520 | // setup AK8975 (0x0E) as Slave 0 in read mode |
| JojoS | 0:ab00d9bcd00d | 521 | DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); |
| JojoS | 0:ab00d9bcd00d | 522 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_ADDR, 0x8E); |
| JojoS | 0:ab00d9bcd00d | 523 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_REG, 0x01); |
| JojoS | 0:ab00d9bcd00d | 524 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV0_CTRL, 0xDA); |
| JojoS | 0:ab00d9bcd00d | 525 | |
| JojoS | 0:ab00d9bcd00d | 526 | // setup AK8975 (0x0E) as Slave 2 in write mode |
| JojoS | 0:ab00d9bcd00d | 527 | DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); |
| JojoS | 0:ab00d9bcd00d | 528 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_ADDR, 0x0E); |
| JojoS | 0:ab00d9bcd00d | 529 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_REG, 0x0A); |
| JojoS | 0:ab00d9bcd00d | 530 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_CTRL, 0x81); |
| JojoS | 0:ab00d9bcd00d | 531 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV2_DO, 0x01); |
| JojoS | 0:ab00d9bcd00d | 532 | |
| JojoS | 0:ab00d9bcd00d | 533 | // setup I2C timing/delay control |
| JojoS | 0:ab00d9bcd00d | 534 | DEBUG_PRINTLN(F("Setting up slave access delay...")); |
| JojoS | 0:ab00d9bcd00d | 535 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_SLV4_CTRL, 0x18); |
| JojoS | 0:ab00d9bcd00d | 536 | I2Cdev::writeByte(0x68, MPU9150_RA_I2C_MST_DELAY_CTRL, 0x05); |
| JojoS | 0:ab00d9bcd00d | 537 | |
| JojoS | 0:ab00d9bcd00d | 538 | // enable interrupts |
| JojoS | 0:ab00d9bcd00d | 539 | DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); |
| JojoS | 0:ab00d9bcd00d | 540 | I2Cdev::writeByte(0x68, MPU9150_RA_INT_PIN_CFG, 0x00); |
| JojoS | 0:ab00d9bcd00d | 541 | |
| JojoS | 0:ab00d9bcd00d | 542 | // enable I2C master mode and reset DMP/FIFO |
| JojoS | 0:ab00d9bcd00d | 543 | DEBUG_PRINTLN(F("Enabling I2C master mode...")); |
| JojoS | 0:ab00d9bcd00d | 544 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20); |
| JojoS | 0:ab00d9bcd00d | 545 | DEBUG_PRINTLN(F("Resetting FIFO...")); |
| JojoS | 0:ab00d9bcd00d | 546 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x24); |
| JojoS | 0:ab00d9bcd00d | 547 | DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); |
| JojoS | 0:ab00d9bcd00d | 548 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0x20); |
| JojoS | 0:ab00d9bcd00d | 549 | DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); |
| JojoS | 0:ab00d9bcd00d | 550 | I2Cdev::writeByte(0x68, MPU9150_RA_USER_CTRL, 0xE8); |
| JojoS | 0:ab00d9bcd00d | 551 | |
| JojoS | 0:ab00d9bcd00d | 552 | DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 553 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 554 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 555 | DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 556 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 557 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 558 | DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 559 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 560 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 561 | DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 562 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 563 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 564 | DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 565 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 566 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 567 | DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 568 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 569 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 570 | DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 571 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 572 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 573 | |
| JojoS | 0:ab00d9bcd00d | 574 | DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 575 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 576 | readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 577 | #ifdef DEBUG |
| JojoS | 0:ab00d9bcd00d | 578 | DEBUG_PRINT(F("Read bytes: ")); |
| JojoS | 0:ab00d9bcd00d | 579 | for (j = 0; j < 4; j++) { |
| JojoS | 0:ab00d9bcd00d | 580 | DEBUG_PRINTF(dmpUpdate[3 + j], HEX); |
| JojoS | 0:ab00d9bcd00d | 581 | DEBUG_PRINT(" "); |
| JojoS | 0:ab00d9bcd00d | 582 | } |
| JojoS | 0:ab00d9bcd00d | 583 | DEBUG_PRINTLN(""); |
| JojoS | 0:ab00d9bcd00d | 584 | #endif |
| JojoS | 0:ab00d9bcd00d | 585 | |
| JojoS | 0:ab00d9bcd00d | 586 | DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 587 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 588 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 589 | DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 590 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 591 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 592 | DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 593 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 594 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 595 | DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 596 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 597 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 598 | DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 599 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 600 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 601 | |
| JojoS | 0:ab00d9bcd00d | 602 | DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); |
| JojoS | 0:ab00d9bcd00d | 603 | while ((fifoCount = getFIFOCount()) < 46); |
| JojoS | 0:ab00d9bcd00d | 604 | DEBUG_PRINTLN(F("Reading FIFO...")); |
| JojoS | 0:ab00d9bcd00d | 605 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes |
| JojoS | 0:ab00d9bcd00d | 606 | DEBUG_PRINTLN(F("Reading interrupt status...")); |
| JojoS | 0:ab00d9bcd00d | 607 | getIntStatus(); |
| JojoS | 0:ab00d9bcd00d | 608 | |
| JojoS | 0:ab00d9bcd00d | 609 | DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 610 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 611 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 612 | |
| JojoS | 0:ab00d9bcd00d | 613 | DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); |
| JojoS | 0:ab00d9bcd00d | 614 | while ((fifoCount = getFIFOCount()) < 48); |
| JojoS | 0:ab00d9bcd00d | 615 | DEBUG_PRINTLN(F("Reading FIFO...")); |
| JojoS | 0:ab00d9bcd00d | 616 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes |
| JojoS | 0:ab00d9bcd00d | 617 | DEBUG_PRINTLN(F("Reading interrupt status...")); |
| JojoS | 0:ab00d9bcd00d | 618 | getIntStatus(); |
| JojoS | 0:ab00d9bcd00d | 619 | DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); |
| JojoS | 0:ab00d9bcd00d | 620 | while ((fifoCount = getFIFOCount()) < 48); |
| JojoS | 0:ab00d9bcd00d | 621 | DEBUG_PRINTLN(F("Reading FIFO...")); |
| JojoS | 0:ab00d9bcd00d | 622 | getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes |
| JojoS | 0:ab00d9bcd00d | 623 | DEBUG_PRINTLN(F("Reading interrupt status...")); |
| JojoS | 0:ab00d9bcd00d | 624 | getIntStatus(); |
| JojoS | 0:ab00d9bcd00d | 625 | |
| JojoS | 0:ab00d9bcd00d | 626 | DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); |
| JojoS | 0:ab00d9bcd00d | 627 | for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); |
| JojoS | 0:ab00d9bcd00d | 628 | writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); |
| JojoS | 0:ab00d9bcd00d | 629 | |
| JojoS | 0:ab00d9bcd00d | 630 | DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); |
| JojoS | 0:ab00d9bcd00d | 631 | setDMPEnabled(false); |
| JojoS | 0:ab00d9bcd00d | 632 | |
| JojoS | 0:ab00d9bcd00d | 633 | DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); |
| JojoS | 0:ab00d9bcd00d | 634 | dmpPacketSize = 48; |
| JojoS | 0:ab00d9bcd00d | 635 | /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { |
| JojoS | 0:ab00d9bcd00d | 636 | return 3; // TODO: proper error code for no memory |
| JojoS | 0:ab00d9bcd00d | 637 | }*/ |
| JojoS | 0:ab00d9bcd00d | 638 | |
| JojoS | 0:ab00d9bcd00d | 639 | DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); |
| JojoS | 0:ab00d9bcd00d | 640 | resetFIFO(); |
| JojoS | 0:ab00d9bcd00d | 641 | getIntStatus(); |
| JojoS | 0:ab00d9bcd00d | 642 | } else { |
| JojoS | 0:ab00d9bcd00d | 643 | DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); |
| JojoS | 0:ab00d9bcd00d | 644 | return 2; // configuration block loading failed |
| JojoS | 0:ab00d9bcd00d | 645 | } |
| JojoS | 0:ab00d9bcd00d | 646 | } else { |
| JojoS | 0:ab00d9bcd00d | 647 | DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); |
| JojoS | 0:ab00d9bcd00d | 648 | return 1; // main binary block loading failed |
| JojoS | 0:ab00d9bcd00d | 649 | } |
| JojoS | 0:ab00d9bcd00d | 650 | return 0; // success |
| JojoS | 0:ab00d9bcd00d | 651 | } |
| JojoS | 0:ab00d9bcd00d | 652 | |
| JojoS | 0:ab00d9bcd00d | 653 | bool MPU9150::dmpPacketAvailable() { |
| JojoS | 0:ab00d9bcd00d | 654 | return getFIFOCount() >= dmpGetFIFOPacketSize(); |
| JojoS | 0:ab00d9bcd00d | 655 | } |
| JojoS | 0:ab00d9bcd00d | 656 | |
| JojoS | 0:ab00d9bcd00d | 657 | // uint8_t MPU9150::dmpSetFIFORate(uint8_t fifoRate); |
| JojoS | 0:ab00d9bcd00d | 658 | // uint8_t MPU9150::dmpGetFIFORate(); |
| JojoS | 0:ab00d9bcd00d | 659 | // uint8_t MPU9150::dmpGetSampleStepSizeMS(); |
| JojoS | 0:ab00d9bcd00d | 660 | // uint8_t MPU9150::dmpGetSampleFrequency(); |
| JojoS | 0:ab00d9bcd00d | 661 | // int32_t MPU9150::dmpDecodeTemperature(int8_t tempReg); |
| JojoS | 0:ab00d9bcd00d | 662 | |
| JojoS | 0:ab00d9bcd00d | 663 | //uint8_t MPU9150::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); |
| JojoS | 0:ab00d9bcd00d | 664 | //uint8_t MPU9150::dmpUnregisterFIFORateProcess(inv_obj_func func); |
| JojoS | 0:ab00d9bcd00d | 665 | //uint8_t MPU9150::dmpRunFIFORateProcesses(); |
| JojoS | 0:ab00d9bcd00d | 666 | |
| JojoS | 0:ab00d9bcd00d | 667 | // uint8_t MPU9150::dmpSendQuaternion(uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 668 | // uint8_t MPU9150::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 669 | // uint8_t MPU9150::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 670 | // uint8_t MPU9150::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 671 | // uint8_t MPU9150::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 672 | // uint8_t MPU9150::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 673 | // uint8_t MPU9150::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 674 | // uint8_t MPU9150::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 675 | // uint8_t MPU9150::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 676 | // uint8_t MPU9150::dmpSendPacketNumber(uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 677 | // uint8_t MPU9150::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 678 | // uint8_t MPU9150::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); |
| JojoS | 0:ab00d9bcd00d | 679 | |
| JojoS | 0:ab00d9bcd00d | 680 | uint8_t MPU9150::dmpGetAccel(int32_t *data, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 681 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 682 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 683 | data[0] = ((packet[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]); |
| JojoS | 0:ab00d9bcd00d | 684 | data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]); |
| JojoS | 0:ab00d9bcd00d | 685 | data[2] = ((packet[42] << 24) + (packet[43] << 16) + (packet[44] << 8) + packet[45]); |
| JojoS | 0:ab00d9bcd00d | 686 | return 0; |
| JojoS | 0:ab00d9bcd00d | 687 | } |
| JojoS | 0:ab00d9bcd00d | 688 | uint8_t MPU9150::dmpGetAccel(int16_t *data, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 689 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 690 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 691 | data[0] = (packet[34] << 8) + packet[35]; |
| JojoS | 0:ab00d9bcd00d | 692 | data[1] = (packet[38] << 8) + packet[39]; |
| JojoS | 0:ab00d9bcd00d | 693 | data[2] = (packet[42] << 8) + packet[43]; |
| JojoS | 0:ab00d9bcd00d | 694 | return 0; |
| JojoS | 0:ab00d9bcd00d | 695 | } |
| JojoS | 0:ab00d9bcd00d | 696 | uint8_t MPU9150::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 697 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 698 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 699 | v -> x = (packet[34] << 8) + packet[35]; |
| JojoS | 0:ab00d9bcd00d | 700 | v -> y = (packet[38] << 8) + packet[39]; |
| JojoS | 0:ab00d9bcd00d | 701 | v -> z = (packet[42] << 8) + packet[43]; |
| JojoS | 0:ab00d9bcd00d | 702 | return 0; |
| JojoS | 0:ab00d9bcd00d | 703 | } |
| JojoS | 0:ab00d9bcd00d | 704 | uint8_t MPU9150::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 705 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 706 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 707 | data[0] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); |
| JojoS | 0:ab00d9bcd00d | 708 | data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); |
| JojoS | 0:ab00d9bcd00d | 709 | data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); |
| JojoS | 0:ab00d9bcd00d | 710 | data[3] = ((packet[12] << 24) + (packet[13] << 16) + (packet[14] << 8) + packet[15]); |
| JojoS | 0:ab00d9bcd00d | 711 | return 0; |
| JojoS | 0:ab00d9bcd00d | 712 | } |
| JojoS | 0:ab00d9bcd00d | 713 | uint8_t MPU9150::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 714 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 715 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 716 | data[0] = ((packet[0] << 8) + packet[1]); |
| JojoS | 0:ab00d9bcd00d | 717 | data[1] = ((packet[4] << 8) + packet[5]); |
| JojoS | 0:ab00d9bcd00d | 718 | data[2] = ((packet[8] << 8) + packet[9]); |
| JojoS | 0:ab00d9bcd00d | 719 | data[3] = ((packet[12] << 8) + packet[13]); |
| JojoS | 0:ab00d9bcd00d | 720 | return 0; |
| JojoS | 0:ab00d9bcd00d | 721 | } |
| JojoS | 0:ab00d9bcd00d | 722 | uint8_t MPU9150::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 723 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 724 | int16_t qI[4]; |
| JojoS | 0:ab00d9bcd00d | 725 | uint8_t status = dmpGetQuaternion(qI, packet); |
| JojoS | 0:ab00d9bcd00d | 726 | if (status == 0) { |
| JojoS | 0:ab00d9bcd00d | 727 | q -> w = (float)qI[0] / 16384.0f; |
| JojoS | 0:ab00d9bcd00d | 728 | q -> x = (float)qI[1] / 16384.0f; |
| JojoS | 0:ab00d9bcd00d | 729 | q -> y = (float)qI[2] / 16384.0f; |
| JojoS | 0:ab00d9bcd00d | 730 | q -> z = (float)qI[3] / 16384.0f; |
| JojoS | 0:ab00d9bcd00d | 731 | return 0; |
| JojoS | 0:ab00d9bcd00d | 732 | } |
| JojoS | 0:ab00d9bcd00d | 733 | return status; // int16 return value, indicates error if this line is reached |
| JojoS | 0:ab00d9bcd00d | 734 | } |
| JojoS | 0:ab00d9bcd00d | 735 | // uint8_t MPU9150::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 736 | // uint8_t MPU9150::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 737 | uint8_t MPU9150::dmpGetGyro(int32_t *data, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 738 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 739 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 740 | data[0] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); |
| JojoS | 0:ab00d9bcd00d | 741 | data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); |
| JojoS | 0:ab00d9bcd00d | 742 | data[2] = ((packet[24] << 24) + (packet[25] << 16) + (packet[26] << 8) + packet[27]); |
| JojoS | 0:ab00d9bcd00d | 743 | return 0; |
| JojoS | 0:ab00d9bcd00d | 744 | } |
| JojoS | 0:ab00d9bcd00d | 745 | uint8_t MPU9150::dmpGetGyro(int16_t *data, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 746 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 747 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 748 | data[0] = (packet[16] << 8) + packet[17]; |
| JojoS | 0:ab00d9bcd00d | 749 | data[1] = (packet[20] << 8) + packet[21]; |
| JojoS | 0:ab00d9bcd00d | 750 | data[2] = (packet[24] << 8) + packet[25]; |
| JojoS | 0:ab00d9bcd00d | 751 | return 0; |
| JojoS | 0:ab00d9bcd00d | 752 | } |
| JojoS | 0:ab00d9bcd00d | 753 | uint8_t MPU9150::dmpGetMag(int16_t *data, const uint8_t* packet) { |
| JojoS | 0:ab00d9bcd00d | 754 | // TODO: accommodate different arrangements of sent data (ONLY default supported now) |
| JojoS | 0:ab00d9bcd00d | 755 | if (packet == 0) packet = dmpPacketBuffer; |
| JojoS | 0:ab00d9bcd00d | 756 | data[0] = (packet[28] << 8) + packet[29]; |
| JojoS | 0:ab00d9bcd00d | 757 | data[1] = (packet[30] << 8) + packet[31]; |
| JojoS | 0:ab00d9bcd00d | 758 | data[2] = (packet[32] << 8) + packet[33]; |
| JojoS | 0:ab00d9bcd00d | 759 | return 0; |
| JojoS | 0:ab00d9bcd00d | 760 | } |
| JojoS | 0:ab00d9bcd00d | 761 | // uint8_t MPU9150::dmpSetLinearAccelFilterCoefficient(float coef); |
| JojoS | 0:ab00d9bcd00d | 762 | // uint8_t MPU9150::dmpGetLinearAccel(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 763 | uint8_t MPU9150::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { |
| JojoS | 0:ab00d9bcd00d | 764 | // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) |
| JojoS | 0:ab00d9bcd00d | 765 | v -> x = vRaw -> x - gravity -> x*4096; |
| JojoS | 0:ab00d9bcd00d | 766 | v -> y = vRaw -> y - gravity -> y*4096; |
| JojoS | 0:ab00d9bcd00d | 767 | v -> z = vRaw -> z - gravity -> z*4096; |
| JojoS | 0:ab00d9bcd00d | 768 | return 0; |
| JojoS | 0:ab00d9bcd00d | 769 | } |
| JojoS | 0:ab00d9bcd00d | 770 | // uint8_t MPU9150::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 771 | uint8_t MPU9150::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { |
| JojoS | 0:ab00d9bcd00d | 772 | // rotate measured 3D acceleration vector into original state |
| JojoS | 0:ab00d9bcd00d | 773 | // frame of reference based on orientation quaternion |
| JojoS | 0:ab00d9bcd00d | 774 | memcpy(v, vReal, sizeof(VectorInt16)); |
| JojoS | 0:ab00d9bcd00d | 775 | v -> rotate(q); |
| JojoS | 0:ab00d9bcd00d | 776 | return 0; |
| JojoS | 0:ab00d9bcd00d | 777 | } |
| JojoS | 0:ab00d9bcd00d | 778 | // uint8_t MPU9150::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 779 | // uint8_t MPU9150::dmpGetGyroSensor(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 780 | // uint8_t MPU9150::dmpGetControlData(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 781 | // uint8_t MPU9150::dmpGetTemperature(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 782 | // uint8_t MPU9150::dmpGetGravity(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 783 | uint8_t MPU9150::dmpGetGravity(VectorFloat *v, Quaternion *q) { |
| JojoS | 0:ab00d9bcd00d | 784 | v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); |
| JojoS | 0:ab00d9bcd00d | 785 | v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); |
| JojoS | 0:ab00d9bcd00d | 786 | v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; |
| JojoS | 0:ab00d9bcd00d | 787 | return 0; |
| JojoS | 0:ab00d9bcd00d | 788 | } |
| JojoS | 0:ab00d9bcd00d | 789 | // uint8_t MPU9150::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 790 | // uint8_t MPU9150::dmpGetQuantizedAccel(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 791 | // uint8_t MPU9150::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 792 | // uint8_t MPU9150::dmpGetEIS(long *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 793 | |
| JojoS | 0:ab00d9bcd00d | 794 | uint8_t MPU9150::dmpGetEuler(float *data, Quaternion *q) { |
| JojoS | 0:ab00d9bcd00d | 795 | 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 |
| JojoS | 0:ab00d9bcd00d | 796 | data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta |
| JojoS | 0:ab00d9bcd00d | 797 | 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 |
| JojoS | 0:ab00d9bcd00d | 798 | return 0; |
| JojoS | 0:ab00d9bcd00d | 799 | } |
| JojoS | 0:ab00d9bcd00d | 800 | uint8_t MPU9150::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { |
| JojoS | 0:ab00d9bcd00d | 801 | // yaw: (about Z axis) |
| JojoS | 0:ab00d9bcd00d | 802 | data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); |
| JojoS | 0:ab00d9bcd00d | 803 | // pitch: (nose up/down, about Y axis) |
| JojoS | 0:ab00d9bcd00d | 804 | data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); |
| JojoS | 0:ab00d9bcd00d | 805 | // roll: (tilt left/right, about X axis) |
| JojoS | 0:ab00d9bcd00d | 806 | data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); |
| JojoS | 0:ab00d9bcd00d | 807 | return 0; |
| JojoS | 0:ab00d9bcd00d | 808 | } |
| JojoS | 0:ab00d9bcd00d | 809 | |
| JojoS | 0:ab00d9bcd00d | 810 | // uint8_t MPU9150::dmpGetAccelFloat(float *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 811 | // uint8_t MPU9150::dmpGetQuaternionFloat(float *data, const uint8_t* packet); |
| JojoS | 0:ab00d9bcd00d | 812 | |
| JojoS | 0:ab00d9bcd00d | 813 | uint8_t MPU9150::dmpProcessFIFOPacket(const unsigned char *dmpData) { |
| JojoS | 0:ab00d9bcd00d | 814 | /*for (uint8_t k = 0; k < dmpPacketSize; k++) { |
| JojoS | 0:ab00d9bcd00d | 815 | if (dmpData[k] < 0x10) Serial.print("0"); |
| JojoS | 0:ab00d9bcd00d | 816 | Serial.print(dmpData[k], HEX); |
| JojoS | 0:ab00d9bcd00d | 817 | Serial.print(" "); |
| JojoS | 0:ab00d9bcd00d | 818 | } |
| JojoS | 0:ab00d9bcd00d | 819 | Serial.print("\n");*/ |
| JojoS | 0:ab00d9bcd00d | 820 | //Serial.println((uint16_t)dmpPacketBuffer); |
| JojoS | 0:ab00d9bcd00d | 821 | return 0; |
| JojoS | 0:ab00d9bcd00d | 822 | } |
| JojoS | 0:ab00d9bcd00d | 823 | uint8_t MPU9150::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { |
| JojoS | 0:ab00d9bcd00d | 824 | uint8_t status; |
| JojoS | 0:ab00d9bcd00d | 825 | uint8_t buf[dmpPacketSize]; |
| JojoS | 0:ab00d9bcd00d | 826 | for (uint8_t i = 0; i < numPackets; i++) { |
| JojoS | 0:ab00d9bcd00d | 827 | // read packet from FIFO |
| JojoS | 0:ab00d9bcd00d | 828 | getFIFOBytes(buf, dmpPacketSize); |
| JojoS | 0:ab00d9bcd00d | 829 | |
| JojoS | 0:ab00d9bcd00d | 830 | // process packet |
| JojoS | 0:ab00d9bcd00d | 831 | if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; |
| JojoS | 0:ab00d9bcd00d | 832 | |
| JojoS | 0:ab00d9bcd00d | 833 | // increment external process count variable, if supplied |
| JojoS | 0:ab00d9bcd00d | 834 | if (processed != 0) *processed++; |
| JojoS | 0:ab00d9bcd00d | 835 | } |
| JojoS | 0:ab00d9bcd00d | 836 | return 0; |
| JojoS | 0:ab00d9bcd00d | 837 | } |
| JojoS | 0:ab00d9bcd00d | 838 | |
| JojoS | 0:ab00d9bcd00d | 839 | // uint8_t MPU9150::dmpSetFIFOProcessedCallback(void (*func) (void)); |
| JojoS | 0:ab00d9bcd00d | 840 | |
| JojoS | 0:ab00d9bcd00d | 841 | // uint8_t MPU9150::dmpInitFIFOParam(); |
| JojoS | 0:ab00d9bcd00d | 842 | // uint8_t MPU9150::dmpCloseFIFO(); |
| JojoS | 0:ab00d9bcd00d | 843 | // uint8_t MPU9150::dmpSetGyroDataSource(uint_fast8_t source); |
| JojoS | 0:ab00d9bcd00d | 844 | // uint8_t MPU9150::dmpDecodeQuantizedAccel(); |
| JojoS | 0:ab00d9bcd00d | 845 | // uint32_t MPU9150::dmpGetGyroSumOfSquare(); |
| JojoS | 0:ab00d9bcd00d | 846 | // uint32_t MPU9150::dmpGetAccelSumOfSquare(); |
| JojoS | 0:ab00d9bcd00d | 847 | // void MPU9150::dmpOverrideQuaternion(long *q); |
| JojoS | 0:ab00d9bcd00d | 848 | uint16_t MPU9150::dmpGetFIFOPacketSize() { |
| JojoS | 0:ab00d9bcd00d | 849 | return dmpPacketSize; |
| JojoS | 0:ab00d9bcd00d | 850 | } |
| JojoS | 0:ab00d9bcd00d | 851 | |
| JojoS | 0:ab00d9bcd00d | 852 | #endif /* _MPU9150_9AXIS_MOTIONAPPS41_H_ */ |