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