API tools faq
paste
Advertisement
Guest User

MPU6050_6Axis_MotionApps20.h

a guest
Oct 3rd, 2015
349
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
text 41.10 KB | None | 0 0
raw download clone embed print report
  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)
  3. // 5/20/2013 by Jeff Rowberg <[email protected]>
  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"
  43.  
  44. // Tom Carpenter's conditional PROGMEM code
  45. // http://forum.arduino.cc/index.php?topic=129407.0
  46. #ifndef __arm__
  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
  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)
  133. const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = {
  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!
  273. const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = {
  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.  
  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.  
  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.  
  315. const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = {
  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();
  329. delay(30); // wait after reset
  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. uint8_t hwRevision = readMemoryByte();
  348. DEBUG_PRINT(F("Revision @ user[16][6] = "));
  349. DEBUG_PRINTLNF(hwRevision, HEX);
  350. DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
  351. setMemoryBank(0, false, false);
  352.  
  353. // check OTP bank valid
  354. DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
  355. uint8_t otpValid = getOTPBankValid();
  356. DEBUG_PRINT(F("OTP bank is "));
  357. DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!"));
  358.  
  359. // get X/Y/Z gyro offsets
  360. DEBUG_PRINTLN(F("Reading gyro offset TC values..."));
  361. int8_t xgOffsetTC = getXGyroOffsetTC();
  362. int8_t ygOffsetTC = getYGyroOffsetTC();
  363. int8_t zgOffsetTC = getZGyroOffsetTC();
  364. DEBUG_PRINT(F("X gyro offset = "));
  365. DEBUG_PRINTLN(xgOffsetTC);
  366. DEBUG_PRINT(F("Y gyro offset = "));
  367. DEBUG_PRINTLN(ygOffsetTC);
  368. DEBUG_PRINT(F("Z gyro offset = "));
  369. DEBUG_PRINTLN(zgOffsetTC);
  370.  
  371. // setup weird slave stuff (?)
  372. DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
  373. setSlaveAddress(0, 0x7F);
  374. DEBUG_PRINTLN(F("Disabling I2C Master mode..."));
  375. setI2CMasterModeEnabled(false);
  376. DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)..."));
  377. setSlaveAddress(0, 0x68);
  378. DEBUG_PRINTLN(F("Resetting I2C Master control..."));
  379. resetI2CMaster();
  380. delay(20);
  381.  
  382. // load DMP code into memory banks
  383. DEBUG_PRINT(F("Writing DMP code to MPU memory banks ("));
  384. DEBUG_PRINT(MPU6050_DMP_CODE_SIZE);
  385. DEBUG_PRINTLN(F(" bytes)"));
  386. if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
  387. DEBUG_PRINTLN(F("Success! DMP code written and verified."));
  388.  
  389. // write DMP configuration
  390. DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks ("));
  391. DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE);
  392. DEBUG_PRINTLN(F(" bytes in config def)"));
  393. if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
  394. DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
  395.  
  396. DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
  397. setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
  398.  
  399. DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
  400. setIntEnabled(0x12);
  401.  
  402. DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
  403. setRate(4); // 1khz / (1 + 4) = 200 Hz
  404.  
  405. DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
  406. setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
  407.  
  408. DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
  409. setDLPFMode(MPU6050_DLPF_BW_42);
  410.  
  411. DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
  412. setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
  413.  
  414. DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
  415. setDMPConfig1(0x03);
  416. setDMPConfig2(0x00);
  417.  
  418. DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
  419. setOTPBankValid(false);
  420.  
  421. DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
  422. setXGyroOffsetTC(xgOffsetTC);
  423. setYGyroOffsetTC(ygOffsetTC);
  424. setZGyroOffsetTC(zgOffsetTC);
  425.  
  426. //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero..."));
  427. //setXGyroOffset(0);
  428. //setYGyroOffset(0);
  429. //setZGyroOffset(0);
  430.  
  431. DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)..."));
  432. uint8_t dmpUpdate[16], j;
  433. uint16_t pos = 0;
  434. for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
  435. writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
  436.  
  437. DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)..."));
  438. for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
  439. writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
  440.  
  441. DEBUG_PRINTLN(F("Resetting FIFO..."));
  442. resetFIFO();
  443.  
  444. DEBUG_PRINTLN(F("Reading FIFO count..."));
  445. uint16_t fifoCount = getFIFOCount();
  446. uint8_t fifoBuffer[128];
  447.  
  448. DEBUG_PRINT(F("Current FIFO count="));
  449. DEBUG_PRINTLN(fifoCount);
  450. getFIFOBytes(fifoBuffer, fifoCount);
  451.  
  452. DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
  453. setMotionDetectionThreshold(2);
  454.  
  455. DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
  456. setZeroMotionDetectionThreshold(156);
  457.  
  458. DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
  459. setMotionDetectionDuration(80);
  460.  
  461. DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
  462. setZeroMotionDetectionDuration(0);
  463.  
  464. DEBUG_PRINTLN(F("Resetting FIFO..."));
  465. resetFIFO();
  466.  
  467. DEBUG_PRINTLN(F("Enabling FIFO..."));
  468. setFIFOEnabled(true);
  469.  
  470. DEBUG_PRINTLN(F("Enabling DMP..."));
  471. setDMPEnabled(true);
  472.  
  473. DEBUG_PRINTLN(F("Resetting DMP..."));
  474. resetDMP();
  475.  
  476. DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)..."));
  477. for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
  478. writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
  479.  
  480. DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)..."));
  481. for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
  482. writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
  483.  
  484. DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)..."));
  485. for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
  486. writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
  487.  
  488. DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
  489. while ((fifoCount = getFIFOCount()) < 3);
  490.  
  491. DEBUG_PRINT(F("Current FIFO count="));
  492. DEBUG_PRINTLN(fifoCount);
  493. DEBUG_PRINTLN(F("Reading FIFO data..."));
  494. getFIFOBytes(fifoBuffer, fifoCount);
  495.  
  496. DEBUG_PRINTLN(F("Reading interrupt status..."));
  497. uint8_t mpuIntStatus = getIntStatus();
  498.  
  499. DEBUG_PRINT(F("Current interrupt status="));
  500. DEBUG_PRINTLNF(mpuIntStatus, HEX);
  501.  
  502. DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)..."));
  503. for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
  504. readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
  505.  
  506. DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
  507. while ((fifoCount = getFIFOCount()) < 3);
  508.  
  509. DEBUG_PRINT(F("Current FIFO count="));
  510. DEBUG_PRINTLN(fifoCount);
  511.  
  512. DEBUG_PRINTLN(F("Reading FIFO data..."));
  513. getFIFOBytes(fifoBuffer, fifoCount);
  514.  
  515. DEBUG_PRINTLN(F("Reading interrupt status..."));
  516. mpuIntStatus = getIntStatus();
  517.  
  518. DEBUG_PRINT(F("Current interrupt status="));
  519. DEBUG_PRINTLNF(mpuIntStatus, HEX);
  520.  
  521. DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
  522. for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]);
  523. writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]);
  524.  
  525. DEBUG_PRINTLN(F("DMP is good to go! Finally."));
  526.  
  527. DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
  528. setDMPEnabled(false);
  529.  
  530. DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
  531. dmpPacketSize = 42;
  532. /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
  533. return 3; // TODO: proper error code for no memory
  534. }*/
  535.  
  536. DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time..."));
  537. resetFIFO();
  538. getIntStatus();
  539. } else {
  540. DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
  541. return 2; // configuration block loading failed
  542. }
  543. } else {
  544. DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
  545. return 1; // main binary block loading failed
  546. }
  547. return 0; // success
  548. }
  549.  
  550. bool MPU6050::dmpPacketAvailable() {
  551. return getFIFOCount() >= dmpGetFIFOPacketSize();
  552. }
  553.  
  554. // uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate);
  555. // uint8_t MPU6050::dmpGetFIFORate();
  556. // uint8_t MPU6050::dmpGetSampleStepSizeMS();
  557. // uint8_t MPU6050::dmpGetSampleFrequency();
  558. // int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg);
  559.  
  560. //uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority);
  561. //uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func);
  562. //uint8_t MPU6050::dmpRunFIFORateProcesses();
  563.  
  564. // uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy);
  565. // uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy);
  566. // uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy);
  567. // uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy);
  568. // uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy);
  569. // uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy);
  570. // uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
  571. // uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy);
  572. // uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy);
  573. // uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy);
  574. // uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy);
  575. // uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy);
  576.  
  577. uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) {
  578. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  579. if (packet == 0) packet = dmpPacketBuffer;
  580. data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]);
  581. data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]);
  582. data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]);
  583. return 0;
  584. }
  585. uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) {
  586. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  587. if (packet == 0) packet = dmpPacketBuffer;
  588. data[0] = (packet[28] << 8) | packet[29];
  589. data[1] = (packet[32] << 8) | packet[33];
  590. data[2] = (packet[36] << 8) | packet[37];
  591. return 0;
  592. }
  593. uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) {
  594. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  595. if (packet == 0) packet = dmpPacketBuffer;
  596. v -> x = (packet[28] << 8) | packet[29];
  597. v -> y = (packet[32] << 8) | packet[33];
  598. v -> z = (packet[36] << 8) | packet[37];
  599. return 0;
  600. }
  601. uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) {
  602. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  603. if (packet == 0) packet = dmpPacketBuffer;
  604. data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]);
  605. data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]);
  606. data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]);
  607. data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]);
  608. return 0;
  609. }
  610. uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) {
  611. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  612. if (packet == 0) packet = dmpPacketBuffer;
  613. data[0] = ((packet[0] << 8) | packet[1]);
  614. data[1] = ((packet[4] << 8) | packet[5]);
  615. data[2] = ((packet[8] << 8) | packet[9]);
  616. data[3] = ((packet[12] << 8) | packet[13]);
  617. return 0;
  618. }
  619. uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) {
  620. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  621. int16_t qI[4];
  622. uint8_t status = dmpGetQuaternion(qI, packet);
  623. if (status == 0) {
  624. q -> w = (float)qI[0] / 16384.0f;
  625. q -> x = (float)qI[1] / 16384.0f;
  626. q -> y = (float)qI[2] / 16384.0f;
  627. q -> z = (float)qI[3] / 16384.0f;
  628. return 0;
  629. }
  630. return status; // int16 return value, indicates error if this line is reached
  631. }
  632. // uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet);
  633. // uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet);
  634. uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) {
  635. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  636. if (packet == 0) packet = dmpPacketBuffer;
  637. data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]);
  638. data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]);
  639. data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]);
  640. return 0;
  641. }
  642. uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) {
  643. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  644. if (packet == 0) packet = dmpPacketBuffer;
  645. data[0] = (packet[16] << 8) | packet[17];
  646. data[1] = (packet[20] << 8) | packet[21];
  647. data[2] = (packet[24] << 8) | packet[25];
  648. return 0;
  649. }
  650. uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) {
  651. // TODO: accommodate different arrangements of sent data (ONLY default supported now)
  652. if (packet == 0) packet = dmpPacketBuffer;
  653. v -> x = (packet[16] << 8) | packet[17];
  654. v -> y = (packet[20] << 8) | packet[21];
  655. v -> z = (packet[24] << 8) | packet[25];
  656. return 0;
  657. }
  658. // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef);
  659. // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet);
  660. uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) {
  661. // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g)
  662. v -> x = vRaw -> x - gravity -> x*8192;
  663. v -> y = vRaw -> y - gravity -> y*8192;
  664. v -> z = vRaw -> z - gravity -> z*8192;
  665. return 0;
  666. }
  667. // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet);
  668. uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) {
  669. // rotate measured 3D acceleration vector into original state
  670. // frame of reference based on orientation quaternion
  671. memcpy(v, vReal, sizeof(VectorInt16));
  672. v -> rotate(q);
  673. return 0;
  674. }
  675. // uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet);
  676. // uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet);
  677. // uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet);
  678. // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet);
  679. // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet);
  680. uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) {
  681. v -> x = 2 * (q -> x*q -> z - q -> w*q -> y);
  682. v -> y = 2 * (q -> w*q -> x + q -> y*q -> z);
  683. v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z;
  684. return 0;
  685. }
  686. // uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet);
  687. // uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet);
  688. // uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet);
  689. // uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet);
  690.  
  691. uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) {
  692. 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
  693. data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta
  694. 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
  695. return 0;
  696. }
  697. uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) {
  698. // yaw: (about Z axis)
  699. data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1);
  700. // pitch: (nose up/down, about Y axis)
  701. data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z));
  702. // roll: (tilt left/right, about X axis)
  703. data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z));
  704. return 0;
  705. }
  706.  
  707. // uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet);
  708. // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet);
  709.  
  710. uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) {
  711. /*for (uint8_t k = 0; k < dmpPacketSize; k++) {
  712. if (dmpData[k] < 0x10) Serial.print("0");
  713. Serial.print(dmpData[k], HEX);
  714. Serial.print(" ");
  715. }
  716. Serial.print("\n");*/
  717. //Serial.println((uint16_t)dmpPacketBuffer);
  718. return 0;
  719. }
  720. uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) {
  721. uint8_t status;
  722. uint8_t buf[dmpPacketSize];
  723. for (uint8_t i = 0; i < numPackets; i++) {
  724. // read packet from FIFO
  725. getFIFOBytes(buf, dmpPacketSize);
  726.  
  727. // process packet
  728. if ((status = dmpProcessFIFOPacket(buf)) > 0) return status;
  729.  
  730. // increment external process count variable, if supplied
  731. if (processed != 0) *processed++;
  732. }
  733. return 0;
  734. }
  735.  
  736. // uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void));
  737.  
  738. // uint8_t MPU6050::dmpInitFIFOParam();
  739. // uint8_t MPU6050::dmpCloseFIFO();
  740. // uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source);
  741. // uint8_t MPU6050::dmpDecodeQuantizedAccel();
  742. // uint32_t MPU6050::dmpGetGyroSumOfSquare();
  743. // uint32_t MPU6050::dmpGetAccelSumOfSquare();
  744. // void MPU6050::dmpOverrideQuaternion(long *q);
  745. uint16_t MPU6050::dmpGetFIFOPacketSize() {
  746. return dmpPacketSize;
  747. }
  748.  
  749. #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement
Public Pastes
Advertisement
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!