p kj / Mbed 2 deprecated LPC824-BalanceCar

Microduino

Dependencies:   mbed

Fork of BalanceCar by Li Weiyi

JJ_MPU6050_DMP_6Axis.h@0:a4d8f5b3c546, 2016-06-04 (annotated)

Committer:
lixianyu
Date:
Sat Jun 04 03:16:52 2016 +0000
Revision:
0:a4d8f5b3c546
Pass compile!!

Who changed what in which revision?

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