Rolf Ziegler / Mbed 2 deprecated MPU6050-DMP_6DOF

Complete MPU6050dmp example working with processing 2.1 teapot code v1.0 Jan. 2014

Dependencies:   mbed

Fork of MPU6050 by Shundo Kishi

MPU6050_6Axis_MotionApps20.h@6:029b4b8111ef, 2014-01-10 (annotated)

Committer:
rolfz
Date:
Fri Jan 10 17:01:28 2014 +0000
Revision:
6:029b4b8111ef
Parent:
5:7d1bf3ce0053 
Complete MBED example with main code, working with Processing v2.1 Teapot example!

Who changed what in which revision?

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