From 52200414c680681bde204d51b24355e38d369b1b Mon Sep 17 00:00:00 2001 From: Sabas Date: Mon, 18 Dec 2023 11:17:02 -0600 Subject: [PATCH] Updates Motionapps --- src/MPU6050_6Axis_MotionApps20.cpp | 616 +++++++++++ src/MPU6050_6Axis_MotionApps20.h | 688 ++----------- ...6_12.h => MPU6050_6Axis_MotionApps612.cpp} | 74 +- src/MPU6050_6Axis_MotionApps612.h | 153 +++ src/MPU6050_9Axis_MotionApps41.cpp | 887 ++++++++++++++++ src/MPU6050_9Axis_MotionApps41.h | 958 ++---------------- 6 files changed, 1909 insertions(+), 1467 deletions(-) create mode 100644 src/MPU6050_6Axis_MotionApps20.cpp rename src/{MPU6050_6Axis_MotionApps_V6_12.h => MPU6050_6Axis_MotionApps612.cpp} (91%) create mode 100644 src/MPU6050_6Axis_MotionApps612.h create mode 100644 src/MPU6050_9Axis_MotionApps41.cpp mode change 100755 => 100644 src/MPU6050_9Axis_MotionApps41.h diff --git a/src/MPU6050_6Axis_MotionApps20.cpp b/src/MPU6050_6Axis_MotionApps20.cpp new file mode 100644 index 0000000..3a89674 --- /dev/null +++ b/src/MPU6050_6Axis_MotionApps20.cpp @@ -0,0 +1,616 @@ +// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 5/20/2013 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally +// 2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array +// - Simplified dmpInitialize() to accomidate the dmpmemory array alterations + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2021 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 + +#include "MPU6050_6Axis_MotionApps20.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +/* Source is from the InvenSense MotionApps v2 demo code. Original source is + * unavailable, unless you happen to be amazing as decompiling binary by + * hand (in which case, please contact me, and I'm totally serious). + * + * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the + * DMP reverse-engineering he did to help make this bit of wizardry + * possible. + */ + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v2.0 42-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) + +// I Only Changed this by applying all the configuration data and capturing it before startup: +// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it +static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + /* bank # 0 */ + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + /* bank # 1 */ + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + /* bank # 2 */ + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + /* bank # 3 */ + 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, + 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, + 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, + 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, + 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, + 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, + 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, + 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, + 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, + 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, + 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, + 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, + 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, + 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + /* bank # 4 */ + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + /* bank # 5 */ + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, + 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, + 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, + 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, + 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, + 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, + 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, + 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, + /* bank # 6 */ + 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, + 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, + 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, + 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, + 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, + 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, + 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, + 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, + 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, + 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, + 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, + 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, + 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, + 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, + 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, + 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, + /* bank # 7 */ + 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, + 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, + 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, + 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, + 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, + 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, + 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, + 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, + +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default +#endif + +// I Simplified this: +uint8_t MPU6050_6Axis_MotionApps20::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // enable sleep mode and wake cycle + /*Serial.println(F("Enabling sleep mode...")); + setSleepEnabled(true); + Serial.println(F("Enabling wake cycle...")); + setWakeCycleEnabled(true);*/ + + // disable sleep mode + setSleepEnabled(false); + + // get MPU hardware revision + setMemoryBank(0x10, true, true); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLN(readMemoryByte()); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); + + // setup weird slave stuff (?) + DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); + setSlaveAddress(0, 0x7F); + DEBUG_PRINTLN(F("Disabling I2C Master mode...")); + setI2CMasterModeEnabled(false); + DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); + setSlaveAddress(0, 0x68); + DEBUG_PRINTLN(F("Resetting I2C Master control...")); + resetI2CMaster(); + delay(20); + DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); + setClockSource(MPU6050_CLOCK_PLL_ZGYRO); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1<= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetFIFORate(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetSampleFrequency(); +// int32_t MPU6050_6Axis_MotionApps20::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050_6Axis_MotionApps20::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050_6Axis_MotionApps20::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050_6Axis_MotionApps20::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); + data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); + data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[32] << 8) | packet[33]; + data[2] = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[28] << 8) | packet[29]; + v -> y = (packet[32] << 8) | packet[33]; + v -> z = (packet[36] << 8) | packet[37]; + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[16] << 8) | packet[17]; + v -> y = (packet[20] << 8) | packet[21]; + v -> z = (packet[24] << 8) | packet[25]; + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*8192; + v -> y = vRaw -> y - gravity -> y*8192; + v -> z = vRaw -> z - gravity -> z*8192; + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); + return status; +} + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetEuler(float *data, Quaternion *q) { + 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 + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + 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 + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050_6Axis_MotionApps20::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050_6Axis_MotionApps20::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if (gravity -> z < 0) { + if(data[1] > 0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050_6Axis_MotionApps20::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050_6Axis_MotionApps20::dmpProcessFIFOPacket(const unsigned char *dmpData) { + (void)dmpData; // unused parameter + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050_6Axis_MotionApps20::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050_6Axis_MotionApps20::dmpInitFIFOParam(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpCloseFIFO(); +// uint8_t MPU6050_6Axis_MotionApps20::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050_6Axis_MotionApps20::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050_6Axis_MotionApps20::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050_6Axis_MotionApps20::dmpGetAccelSumOfSquare(); +// void MPU6050_6Axis_MotionApps20::dmpOverrideQuaternion(long *q); +uint16_t MPU6050_6Axis_MotionApps20::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} + + + +uint8_t MPU6050_6Axis_MotionApps20::dmpGetCurrentFIFOPacket(uint8_t *data) { // overflow proof + return(GetCurrentFIFOPacket(data, dmpPacketSize)); +} diff --git a/src/MPU6050_6Axis_MotionApps20.h b/src/MPU6050_6Axis_MotionApps20.h index b8fed6c..0dc9379 100644 --- a/src/MPU6050_6Axis_MotionApps20.h +++ b/src/MPU6050_6Axis_MotionApps20.h @@ -1,13 +1,16 @@ -// I2Cdev library collection - MPU6050 I2C device class, 6-axis MotionApps 2.0 implementation +// I2Cdev library collection - MPU6050 I2C device class // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 5/20/2013 by Jeff Rowberg +// 10/3/2011 by Jeff Rowberg // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: -// 2019/07/08 - merged all DMP Firmware configuration items into the dmpMemory array -// - Simplified dmpInitialize() to accomidate the dmpmemory array alterations +// 2021/09/27 - split implementations out of header files, finally // ... - ongoing debug release +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + /* ============================================ I2Cdev device library code is placed under the MIT license Copyright (c) 2012 Jeff Rowberg @@ -35,591 +38,116 @@ THE SOFTWARE. #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ #define _MPU6050_6AXIS_MOTIONAPPS20_H_ -#include "I2Cdev.h" -#include "helper_3dmath.h" - -// MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 +// take ownership of the "MPU6050" typedef +#define I2CDEVLIB_MPU6050_TYPEDEF #include "MPU6050.h" -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifdef __AVR__ - #include -#elif defined(ESP8266) || defined(ESP32) - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include +class MPU6050_6Axis_MotionApps20 : public MPU6050_Base { + public: + MPU6050_6Axis_MotionApps20(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0) : MPU6050_Base(address, wireObj) { } - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x + uint8_t dmpInitialize(); + bool dmpPacketAvailable(); - typedef void prog_void; - typedef char prog_char; - typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -/* Source is from the InvenSense MotionApps v2 demo code. Original source is - * unavailable, unless you happen to be amazing as decompiling binary by - * hand (in which case, please contact me, and I'm totally serious). - * - * Also, I'd like to offer many, many thanks to Noah Zerkin for all of the - * DMP reverse-engineering he did to help make this bit of wizardry - * possible. - */ - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 1929 // dmpMemory[] -#define MPU6050_DMP_CONFIG_SIZE 192 // dmpConfig[] -#define MPU6050_DMP_UPDATES_SIZE 47 // dmpUpdates[] - -/* ================================================================================================ * - | Default MotionApps v2.0 42-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | - | | - | [GYRO Z][ ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | - | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 | - * ================================================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) - -// I Only Changed this by applying all the configuration data and capturing it before startup: -// *** this is a capture of the DMP Firmware after all the messy changes were made so we can just load it -const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { - /* bank # 0 */ - 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x40, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCB, 0x47, 0xA2, 0x20, 0x00, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, - 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, - 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, - /* bank # 1 */ - 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, - 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, - 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x09, 0x23, 0xA1, 0x35, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, - 0x80, 0x00, 0xFF, 0xFF, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, - 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, - /* bank # 2 */ - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x01, 0x00, 0x05, 0x8B, 0xC1, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - /* bank # 3 */ - 0xD8, 0xDC, 0xBA, 0xA2, 0xF1, 0xDE, 0xB2, 0xB8, 0xB4, 0xA8, 0x81, 0x91, 0xF7, 0x4A, 0x90, 0x7F, - 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, - 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, - 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, - 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, - 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, - 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, - 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, 0xF2, 0xAA, 0x4C, 0xCD, 0x6C, 0xA9, 0x0C, - 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, - 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, - 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, - 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, - 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xAF, 0xF0, - 0x00, 0x28, 0x50, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xD9, 0xFA, 0xA3, 0x86, - 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, - 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, - /* bank # 4 */ - 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, - 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, - 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, - 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, - 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, - 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, - 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, - 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, - 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, - 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, - 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, - 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, - /* bank # 5 */ - 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, - 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, - 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, - 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, - 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, - 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, - 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, - 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, - 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0xA8, 0x8A, - 0x9A, 0xF5, 0x20, 0xAA, 0xDA, 0xDF, 0xD8, 0xA8, 0x40, 0xAA, 0xD0, 0xDA, 0xDE, 0xD8, 0xA8, 0x60, - 0xAA, 0xDA, 0xD0, 0xDF, 0xD8, 0xF1, 0x97, 0x86, 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, - 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, 0xB8, 0xB0, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, - 0x28, 0x51, 0x79, 0x1D, 0x30, 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, - 0x78, 0x9B, 0xF1, 0x1A, 0xB0, 0xF0, 0x8A, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x8B, 0x29, 0x51, 0x79, - 0x8A, 0x24, 0x70, 0x59, 0x8B, 0x20, 0x58, 0x71, 0x8A, 0x44, 0x69, 0x38, 0x8B, 0x39, 0x40, 0x68, - 0x8A, 0x64, 0x48, 0x31, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, 0x58, 0x44, 0x68, - /* bank # 6 */ - 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, 0x8C, 0xA8, 0x04, - 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, 0x26, 0x46, 0x66, - 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, 0x64, 0x48, 0x31, - 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, 0x31, 0x48, 0x60, - 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, 0xA8, 0x6E, 0x76, - 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, 0x6E, 0x8A, 0x56, - 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, 0x9D, 0xB8, 0xAD, - 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0x81, 0x91, - 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, - 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, 0xD9, 0x04, 0xAE, - 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, 0x81, 0xAD, 0xD9, - 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, 0xAD, 0xAD, 0xAD, - 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, 0xF3, 0xAC, 0x2E, - 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, 0x30, 0x18, 0xA8, - 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, 0xF2, 0xB0, 0x89, - 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, 0xD8, 0xD8, 0x79, - /* bank # 7 */ - 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, 0xD9, 0x28, 0xD8, - 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, 0x80, 0x25, 0xDA, - 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, 0x3C, 0xF3, 0xAB, - 0x8B, 0xF8, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xFA, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, - 0xDD, 0xF1, 0x20, 0x28, 0x30, 0x38, 0x9A, 0xF1, 0x28, 0x30, 0x38, 0x9D, 0xF1, 0xA3, 0xA3, 0xA3, - 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, - 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0x28, 0x30, 0x38, - 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0x30, 0xDC, - 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xFE, 0xD8, 0xFF, - + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof + + private: + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; }; -#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR -#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x01 // The New instance of the Firmware has this as the default -#endif - -// I Simplified this: -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - reset(); - delay(30); // wait after reset - - // enable sleep mode and wake cycle - /*Serial.println(F("Enabling sleep mode...")); - setSleepEnabled(true); - Serial.println(F("Enabling wake cycle...")); - setWakeCycleEnabled(true);*/ - - // disable sleep mode - setSleepEnabled(false); - - // get MPU hardware revision - setMemoryBank(0x10, true, true); - setMemoryStartAddress(0x06); - DEBUG_PRINTLN(F("Checking hardware revision...")); - DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLN(readMemoryByte()); - DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); - setMemoryBank(0, false, false); - - // check OTP bank valid - DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); - DEBUG_PRINT(F("OTP bank is ")); - DEBUG_PRINTLN(getOTPBankValid() ? F("valid!") : F("invalid!")); - - // setup weird slave stuff (?) - DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); - setSlaveAddress(0, 0x7F); - DEBUG_PRINTLN(F("Disabling I2C Master mode...")); - setI2CMasterModeEnabled(false); - DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); - setSlaveAddress(0, 0x68); - DEBUG_PRINTLN(F("Resetting I2C Master control...")); - resetI2CMaster(); - delay(20); - DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); - setClockSource(MPU6050_IMU::MPU6050_CLOCK_PLL_ZGYRO); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - setIntEnabled((1<<(MPU6050_IMU::MPU6050_INTERRUPT_FIFO_OFLOW_BIT))|(1<<(MPU6050_IMU::MPU6050_INTERRUPT_DMP_INT_BIT))); - - DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); - setRate(4); // 1khz / (1 + 4) = 200 Hz - - DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); - setExternalFrameSync(MPU6050_IMU::MPU6050_EXT_SYNC_TEMP_OUT_L); - - DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); - setDLPFMode(MPU6050_IMU::MPU6050_DLPF_BW_42); - - DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); - setFullScaleGyroRange(MPU6050_IMU::MPU6050_GYRO_FS_2000); - - // load DMP code into memory banks - DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Failed - DEBUG_PRINTLN(F("Success! DMP code written and verified.")); - - // Set the FIFO Rate Divisor int the DMP Firmware Memory - unsigned char dmpUpdate[] = {0x00, MPU6050_DMP_FIFO_RATE_DIVISOR}; - writeMemoryBlock(dmpUpdate, 0x02, 0x02, 0x16); // Lets write the dmpUpdate data to the Firmware image, we have 2 bytes to write in bank 0x02 with the Offset 0x16 - - //write start address MSB into register - setDMPConfig1(0x03); - //write start address LSB into register - setDMPConfig2(0x00); - - DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); - setOTPBankValid(false); - - DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); - setMotionDetectionThreshold(2); - - DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); - setZeroMotionDetectionThreshold(156); - - DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); - setMotionDetectionDuration(80); - - DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); - setZeroMotionDetectionDuration(0); - DEBUG_PRINTLN(F("Enabling FIFO...")); - setFIFOEnabled(true); - - DEBUG_PRINTLN(F("Resetting DMP...")); - resetDMP(); - - DEBUG_PRINTLN(F("DMP is good to go! Finally.")); - - DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); - setDMPEnabled(false); - - DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); - dmpPacketSize = 42; - - DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); - resetFIFO(); - getIntStatus(); - - return 0; // success -} -// Nothing else changed - -bool MPU6050::dmpPacketAvailable() { - return getFIFOCount() >= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[28] << 24) | ((uint32_t)packet[29] << 16) | ((uint32_t)packet[30] << 8) | packet[31]); - data[1] = (((uint32_t)packet[32] << 24) | ((uint32_t)packet[33] << 16) | ((uint32_t)packet[34] << 8) | packet[35]); - data[2] = (((uint32_t)packet[36] << 24) | ((uint32_t)packet[37] << 16) | ((uint32_t)packet[38] << 8) | packet[39]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) | packet[29]; - data[1] = (packet[32] << 8) | packet[33]; - data[2] = (packet[36] << 8) | packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[28] << 8) | packet[29]; - v -> y = (packet[32] << 8) | packet[33]; - v -> z = (packet[36] << 8) | packet[37]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); - data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); - data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[20] << 8) | packet[21]; - data[2] = (packet[24] << 8) | packet[25]; - return 0; -} -uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[16] << 8) | packet[17]; - v -> y = (packet[20] << 8) | packet[21]; - v -> z = (packet[24] << 8) | packet[25]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) - v -> x = vRaw -> x - gravity -> x*8192; - v -> y = vRaw -> y - gravity -> y*8192; - v -> z = vRaw -> z - gravity -> z*8192; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { - /* +1g corresponds to +8192, sensitivity is 2g. */ - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; - data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; - data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); - return status; -} - -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - 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 - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - 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 - return 0; -} - -#ifdef USE_OLD_DMPGETYAWPITCHROLL -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} -#else -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan2(gravity -> y , gravity -> z); - if (gravity -> z < 0) { - if(data[1] > 0) { - data[1] = PI - data[1]; - } else { - data[1] = -PI - data[1]; - } - } - return 0; -} -#endif - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) (*processed)++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} - -void MPU6050::dmpSetFIFOPacketSize(uint16_t packetSize) { - dmpPacketSize = packetSize; -} - -uint8_t MPU6050::dmpGetCurrentFIFOPacket(uint8_t *data) { // overflow proof - return(GetCurrentFIFOPacket(data, dmpPacketSize)); -} +typedef MPU6050_6Axis_MotionApps20 MPU6050; #endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/src/MPU6050_6Axis_MotionApps_V6_12.h b/src/MPU6050_6Axis_MotionApps612.cpp similarity index 91% rename from src/MPU6050_6Axis_MotionApps_V6_12.h rename to src/MPU6050_6Axis_MotionApps612.cpp index f8c0144..653a9e5 100644 --- a/src/MPU6050_6Axis_MotionApps_V6_12.h +++ b/src/MPU6050_6Axis_MotionApps612.cpp @@ -4,11 +4,12 @@ // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: -// 2019/7/10 - I incorporated DMP Firmware Version 6.12 Latest as of today with many features and bug fixes. -// - MPU6050 Registers have not changed just the DMP Image so that full backwards compatibility is present -// - Run-time calibration routine is enabled which calibrates after no motion state is detected -// - once no motion state is detected Calibration completes within 0.5 seconds -// - The Drawback is that the firmware image is larger. +// 2021/09/27 - split implementations out of header files, finally +// 2019/07/10 - I incorporated DMP Firmware Version 6.12 Latest as of today with many features and bug fixes. +// - MPU6050 Registers have not changed just the DMP Image so that full backwards compatibility is present +// - Run-time calibration routine is enabled which calibrates after no motion state is detected +// - once no motion state is detected Calibration completes within 0.5 seconds +// - The Drawback is that the firmware image is larger. // ... - ongoing debug release /* ============================================ @@ -35,23 +36,17 @@ THE SOFTWARE. =============================================== */ -#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ -#define _MPU6050_6AXIS_MOTIONAPPS20_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" - // MotionApps 2.0 DMP implementation, built using the MPU-6050EVB evaluation board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS20 // same definitions Should work with V6.12 +#define MPU6050_INCLUDE_DMP_MOTIONAPPS612 -#include "MPU6050.h" +#include "MPU6050_6Axis_MotionApps612.h" // Tom Carpenter's conditional PROGMEM code // http://forum.arduino.cc/index.php?topic=129407.0 #ifdef __AVR__ #include -#elif defined(ESP8266) || defined(ESP32) - #include +#elif defined(ESP32) + #include #else // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen #ifndef __PGMSPACE_H_ @@ -320,7 +315,7 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { 0xC1, 0xC3, 0xD8, 0xB1, 0xB9, 0xF3, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, 0xA3, 0xDD, 0xF1, 0xB3, 0x8B, 0x8B, 0x8B, 0x8B, 0x8B, 0xB0, 0x87, 0x20, 0x28, 0x30, 0x38, 0xB2, 0x8B, 0xB6, 0x9B, 0xF2, 0xA3, 0xC0, 0xC8, 0xC2, 0xC4, 0xCC, 0xC6, 0xA3, 0xA3, -0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9, +0xA3, 0xF1, 0xB0, 0x87, 0xB5, 0x9A, 0xD8, 0xF3, 0x9B, 0xA3, 0xA3, 0xDC, 0xBA, 0xAC, 0xDF, 0xB9, //Reverted back as packet size changes causing isues... TODO:change 2742 from 0xD8 to 0x20 Including the DMP_FEATURE_TAP -- known issue in which if you do not enable DMP_FEATURE_TAP then the interrupts will be at 200Hz even if fifo rate 0xA3, 0xFE, 0xF2, 0xAB, 0xC4, 0xAA, 0xF1, 0xDF, 0xDF, 0xBB, 0xAF, 0xDF, 0xDF, 0xA3, 0xA3, 0xA3, 0xD8, 0xD8, 0xD8, 0xBB, 0xB3, 0xB7, 0xF1, 0xAA, 0xF9, 0xDA, 0xFF, 0xD9, 0x80, 0x9A, 0xAA, 0x28, 0xB4, 0x80, 0x98, 0xA7, 0x20, 0xB7, 0x97, 0x87, 0xA8, 0x66, 0x88, 0xF0, 0x79, 0x51, 0xF1, 0x90, @@ -351,28 +346,28 @@ const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { // this is the most basic initialization I can create. with the intent that we access the register bytes as few times as needed to get the job done. // for detailed descriptins of all registers and there purpose google "MPU-6000/MPU-6050 Register Map and Descriptions" -uint8_t MPU6050::dmpInitialize() { // Lets get it over with fast Write everything once and set it up necely +uint8_t MPU6050::dmpInitialize(uint8_t rateDivisor = 0x04, uint8_t mpuAddr = 0x68) { // Lets get it over with fast Write everything once and set it up necely uint8_t val; uint16_t ival; // Reset procedure per instructions in the "MPU-6000/MPU-6050 Register Map and Descriptions" page 41 - I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1)); //PWR_MGMT_1: reset with 100ms delay + I2Cdev::writeBit(devAddr,0x6B, 7, (val = 1), wireObj); //PWR_MGMT_1: reset with 100ms delay delay(100); - I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111)); // full SIGNAL_PATH_RESET: with another 100ms delay + I2Cdev::writeBits(devAddr,0x6A, 2, 3, (val = 0b111), wireObj); // full SIGNAL_PATH_RESET: with another 100ms delay delay(100); - I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro - I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00)); // 0000 0000 INT_ENABLE: no Interrupt - I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00)); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead - I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00)); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g - I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80)); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read - I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01)); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro - I2Cdev::writeBytes(devAddr,0x19, 1, &(val = 0x04)); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) - I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01)); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01), wireObj); // 1000 0001 PWR_MGMT_1:Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x00), wireObj); // 0000 0000 INT_ENABLE: no Interrupt + I2Cdev::writeBytes(devAddr,0x23, 1, &(val = 0x00), wireObj); // 0000 0000 MPU FIFO_EN: (all off) Using DMP's FIFO instead + I2Cdev::writeBytes(devAddr,0x1C, 1, &(val = 0x00), wireObj); // 0000 0000 ACCEL_CONFIG: 0 = Accel Full Scale Select: 2g + I2Cdev::writeBytes(devAddr,0x37, 1, &(val = 0x80), wireObj); // 1001 0000 INT_PIN_CFG: ACTL The logic level for int pin is active low. and interrupt status bits are cleared on any read + I2Cdev::writeBytes(devAddr,0x6B, 1, &(val = 0x01), wireObj); // 0000 0001 PWR_MGMT_1: Clock Source Select PLL_X_gyro + I2Cdev::writeBytes(devAddr,0x19, 1, &(val = rateDivisor), wireObj); // 0000 0100 SMPLRT_DIV: Divides the internal sample rate 400Hz ( Sample Rate = Gyroscope Output Rate / (1 + SMPLRT_DIV)) + I2Cdev::writeBytes(devAddr,0x1A, 1, &(val = 0x01), wireObj); // 0000 0001 CONFIG: Digital Low Pass Filter (DLPF) Configuration 188HZ //Im betting this will be the beat if (!writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) return 1; // Loads the DMP image into the MPU6050 Memory // Should Never Fail - I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400)); // DMP Program Start Address - I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18)); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec - I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0)); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo - I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02)); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on - I2Cdev::writeBit(devAddr,0x6A, 2, 1); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte) + I2Cdev::writeWords(devAddr, 0x70, 1, &(ival = 0x0400), wireObj); // DMP Program Start Address + I2Cdev::writeBytes(devAddr,0x1B, 1, &(val = 0x18), wireObj); // 0001 1000 GYRO_CONFIG: 3 = +2000 Deg/sec + I2Cdev::writeBytes(devAddr,0x6A, 1, &(val = 0xC0), wireObj); // 1100 1100 USER_CTRL: Enable Fifo and Reset Fifo + I2Cdev::writeBytes(devAddr,0x38, 1, &(val = 0x02), wireObj); // 0000 0010 INT_ENABLE: RAW_DMP_INT_EN on + I2Cdev::writeBit(devAddr,0x6A, 2, 1, wireObj); // Reset FIFO one last time just for kicks. (MPUi2cWrite reads 0x6A first and only alters 1 bit and then saves the byte) setDMPEnabled(false); // disable DMP for compatibility with the MPU6050 library /* @@ -495,10 +490,10 @@ uint8_t MPU6050::dmpGetGyro(VectorInt16 *v, const uint8_t* packet) { // uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); // uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +8192 in standard DMP FIFO packet, sensitivity is 2g) - v -> x = vRaw -> x - gravity -> x*8192; - v -> y = vRaw -> y - gravity -> y*8192; - v -> z = vRaw -> z - gravity -> z*8192; + // get rid of the gravity component (+1g = +16384 in standard DMP FIFO packet, sensitivity is 2g) + v -> x = vRaw -> x - gravity -> x*16384; + v -> y = vRaw -> y - gravity -> y*16384; + v -> z = vRaw -> z - gravity -> z*16384; return 0; } // uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); @@ -515,13 +510,13 @@ uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Qu // uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); // uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { - /* +1g corresponds to +8192, sensitivity is 2g. */ + /* +1g corresponds to +16384, sensitivity is 2g. */ int16_t qI[4]; uint8_t status = dmpGetQuaternion(qI, packet); data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); return status; } @@ -576,6 +571,7 @@ uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gra // uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { + (void)dmpData; // unused parameter /*for (uint8_t k = 0; k < dmpPacketSize; k++) { if (dmpData[k] < 0x10) Serial.print("0"); Serial.print(dmpData[k], HEX); @@ -619,5 +615,3 @@ uint16_t MPU6050::dmpGetFIFOPacketSize() { uint8_t MPU6050::dmpGetCurrentFIFOPacket(uint8_t *data) { // overflow proof return(GetCurrentFIFOPacket(data, dmpPacketSize)); } - -#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ diff --git a/src/MPU6050_6Axis_MotionApps612.h b/src/MPU6050_6Axis_MotionApps612.h new file mode 100644 index 0000000..5b24bd8 --- /dev/null +++ b/src/MPU6050_6Axis_MotionApps612.h @@ -0,0 +1,153 @@ +// I2Cdev library collection - MPU6050 I2C device class +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 10/3/2011 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally +// ... - ongoing debug release + +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2012 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +#ifndef _MPU6050_6AXIS_MOTIONAPPS612_H_ +#define _MPU6050_6AXIS_MOTIONAPPS612_H_ + +// take ownership of the "MPU6050" typedef +#define I2CDEVLIB_MPU6050_TYPEDEF + +#include "MPU6050.h" + +class MPU6050_6Axis_MotionApps612 : public MPU6050_Base { + public: + MPU6050_6Axis_MotionApps612(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0) : MPU6050_Base(address, wireObj) { } + + uint8_t dmpInitialize(uint8_t rateDivisor = MPU6050_DMP_FIFO_RATE_DIVISOR, uint8_t mpuAddr = 0x68); + bool dmpPacketAvailable(); + + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); + + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); + + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); + + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + uint8_t dmpGetCurrentFIFOPacket(uint8_t *data); // overflow proof + + private: + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; +}; + +typedef MPU6050_6Axis_MotionApps612 MPU6050; + +#endif /* _MPU6050_6AXIS_MOTIONAPPS612_H_ */ diff --git a/src/MPU6050_9Axis_MotionApps41.cpp b/src/MPU6050_9Axis_MotionApps41.cpp new file mode 100644 index 0000000..d85d74a --- /dev/null +++ b/src/MPU6050_9Axis_MotionApps41.cpp @@ -0,0 +1,887 @@ +// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation +// Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) +// 6/18/2012 by Jeff Rowberg +// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib +// +// Changelog: +// 2021/09/27 - split implementations out of header files, finally + +/* ============================================ +I2Cdev device library code is placed under the MIT license +Copyright (c) 2021 Jeff Rowberg + +Permission is hereby granted, free of charge, to any person obtaining a copy +of this software and associated documentation files (the "Software"), to deal +in the Software without restriction, including without limitation the rights +to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +copies of the Software, and to permit persons to whom the Software is +furnished to do so, subject to the following conditions: + +The above copyright notice and this permission notice shall be included in +all copies or substantial portions of the Software. + +THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN +THE SOFTWARE. +=============================================== +*/ + +// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board +#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 + +#include "MPU6050_9Axis_MotionApps41.h" + +// Tom Carpenter's conditional PROGMEM code +// http://forum.arduino.cc/index.php?topic=129407.0 +#ifdef __AVR__ + #include +#elif defined(ESP32) + #include +#else + // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen + #ifndef __PGMSPACE_H_ + #define __PGMSPACE_H_ 1 + #include + + #define PROGMEM + #define PGM_P const char * + #define PSTR(str) (str) + #define F(x) x + + typedef void prog_void; + typedef char prog_char; + //typedef unsigned char prog_uchar; + typedef int8_t prog_int8_t; + typedef uint8_t prog_uint8_t; + typedef int16_t prog_int16_t; + typedef uint16_t prog_uint16_t; + typedef int32_t prog_int32_t; + typedef uint32_t prog_uint32_t; + + #define strcpy_P(dest, src) strcpy((dest), (src)) + #define strcat_P(dest, src) strcat((dest), (src)) + #define strcmp_P(a, b) strcmp((a), (b)) + + #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) + #define pgm_read_word(addr) (*(const unsigned short *)(addr)) + #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) + #define pgm_read_float(addr) (*(const float *)(addr)) + + #define pgm_read_byte_near(addr) pgm_read_byte(addr) + #define pgm_read_word_near(addr) pgm_read_word(addr) + #define pgm_read_dword_near(addr) pgm_read_dword(addr) + #define pgm_read_float_near(addr) pgm_read_float(addr) + #define pgm_read_byte_far(addr) pgm_read_byte(addr) + #define pgm_read_word_far(addr) pgm_read_word(addr) + #define pgm_read_dword_far(addr) pgm_read_dword(addr) + #define pgm_read_float_far(addr) pgm_read_float(addr) + #endif +#endif + +// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. +// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) +// after moving string constants to flash memory storage using the F() +// compiler macro (Arduino IDE 1.0+ required). + +//#define DEBUG +#ifdef DEBUG + #define DEBUG_PRINT(x) Serial.print(x) + #define DEBUG_PRINTF(x, y) Serial.print(x, y) + #define DEBUG_PRINTLN(x) Serial.println(x) + #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) +#else + #define DEBUG_PRINT(x) + #define DEBUG_PRINTF(x, y) + #define DEBUG_PRINTLN(x) + #define DEBUG_PRINTLNF(x, y) +#endif + +#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] +#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] +#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] + +/* ================================================================================================ * + | Default MotionApps v4.1 48-byte FIFO packet structure: | + | | + | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | + | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | + | | + | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | + | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | + * ================================================================================================ */ + +// this block of memory gets written to the MPU on start-up, and it seems +// to be volatile memory, so it has to be done each time (it only takes ~1 +// second though) +static const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { + // bank 0, 256 bytes + 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, + 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, + 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, + 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, + 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, + 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, + 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, + 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, + + // bank 1, 256 bytes + 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, + 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, + 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, + 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, + 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, + + // bank 2, 256 bytes + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, + 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + + // bank 3, 256 bytes + 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, + 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, + 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, + 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, + 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, + 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, + 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, + 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, + 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, + 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, + 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, + 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, + 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, + 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, + 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, + 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, + + // bank 4, 256 bytes + 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, + 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, + 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, + 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, + 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, + 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, + 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, + 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, + 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, + 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, + 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, + 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, + 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, + 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, + + // bank 5, 256 bytes + 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, + 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, + 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, + 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, + 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, + 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, + 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, + 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, + 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, + 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, + 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, + 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, + 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, + 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, + 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, + 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, + + // bank 6, 256 bytes + 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, + 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, + 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, + 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, + 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, + 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, + 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, + 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, + 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, + 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, + 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, + 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, + 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, + 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, + 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, + 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, + + // bank 7, 170 bytes (remainder) + 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, + 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, + 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, + 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, + 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, + 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, + 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, + 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, + 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, + 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, + 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF +}; + +#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR +#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03 +#endif + +static const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { +// BANK OFFSET LENGTH [DATA] + 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? + 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration + 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration + 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration + 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration + 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration + 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration + 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration + + 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration + 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 + 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 + 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 + 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 + 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 + 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 + 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 + 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 + + 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel + 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors + 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion + 0x00, 0xA3, 0x01, 0x00, // ? + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion + 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer + 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? + 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? + // SPECIAL 0x01 = enable interrupts + 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION + 0x07, 0xA7, 0x01, 0xFE, // ? + 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? + 0x07, 0x67, 0x01, 0x9A, // ? + 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo + 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo + 0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate + + // This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, + // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. + // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) + + // It is important to make sure the host processor can keep up with reading and processing + // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. +}; + +const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { + 0x01, 0xB2, 0x02, 0xFF, 0xF5, + 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, + 0x00, 0xA3, 0x01, 0x00, + 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, + 0x01, 0x6A, 0x02, 0x06, 0x00, + 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, + 0x01, 0x08, 0x02, 0x01, 0x20, + 0x01, 0x0A, 0x02, 0x00, 0x4E, + 0x01, 0x02, 0x02, 0xFE, 0xB3, + 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ + 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, + 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, + 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, + 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 +}; + +uint8_t MPU6050_9Axis_MotionApps41::dmpInitialize() { + // reset device + DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); + reset(); + delay(30); // wait after reset + + // disable sleep mode + DEBUG_PRINTLN(F("Disabling sleep mode...")); + setSleepEnabled(false); + + // get MPU product ID + DEBUG_PRINTLN(F("Getting product ID...")); + //uint8_t productID = 0; //getProductID(); + DEBUG_PRINT(F("Product ID = ")); + DEBUG_PRINT(productID); + + // get MPU hardware revision + DEBUG_PRINTLN(F("Selecting user bank 16...")); + setMemoryBank(0x10, true, true); + DEBUG_PRINTLN(F("Selecting memory byte 6...")); + setMemoryStartAddress(0x06); + DEBUG_PRINTLN(F("Checking hardware revision...")); + uint8_t hwRevision = readMemoryByte(); + (void)hwRevision; // suppress unused variable compile warning + DEBUG_PRINT(F("Revision @ user[16][6] = ")); + DEBUG_PRINTLNF(hwRevision, HEX); + DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); + setMemoryBank(0, false, false); + + // check OTP bank valid + DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); + uint8_t otpValid = getOTPBankValid(); + (void)otpValid; // suppress unused variable compile warning + DEBUG_PRINT(F("OTP bank is ")); + DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); + + // get X/Y/Z gyro offsets + DEBUG_PRINTLN(F("Reading gyro offset values...")); + int8_t xgOffset = getXGyroOffset(); + int8_t ygOffset = getYGyroOffset(); + int8_t zgOffset = getZGyroOffset(); + DEBUG_PRINT(F("X gyro offset = ")); + DEBUG_PRINTLN(xgOffset); + DEBUG_PRINT(F("Y gyro offset = ")); + DEBUG_PRINTLN(ygOffset); + DEBUG_PRINT(F("Z gyro offset = ")); + DEBUG_PRINTLN(zgOffset); + + I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer, I2Cdev::readTimeout, wireObj); // ? + + DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); + I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32, wireObj); + + // enable MPU AUX I2C bypass mode + //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); + //setI2CBypassEnabled(true); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00, wireObj); + + DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); + //mag -> setMode(0x0F); + I2Cdev::writeByte(0x0E, 0x0A, 0x0F, wireObj); + + DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); + int8_t asax, asay, asaz; + //mag -> getAdjustment(&asax, &asay, &asaz); + I2Cdev::readBytes(0x0E, 0x10, 3, buffer, I2Cdev::readTimeout, wireObj); + asax = (int8_t)buffer[0]; + asay = (int8_t)buffer[1]; + asaz = (int8_t)buffer[2]; + (void)asax; // suppress unused variable compiler warning + (void)asay; // suppress unused variable compiler warning + (void)asaz; // suppress unused variable compiler warning + DEBUG_PRINT(F("Adjustment X/Y/Z = ")); + DEBUG_PRINT(asax); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINT(asay); + DEBUG_PRINT(F(" / ")); + DEBUG_PRINTLN(asaz); + + DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); + //mag -> setMode(0); + I2Cdev::writeByte(0x0E, 0x0A, 0x00, wireObj); + + // load DMP code into memory banks + DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); + DEBUG_PRINTLN(F(" bytes)")); + if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP code written and verified.")); + + DEBUG_PRINTLN(F("Configuring DMP and related settings...")); + + // write DMP configuration + DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); + DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); + DEBUG_PRINTLN(F(" bytes in config def)")); + if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { + DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); + + DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); + setIntEnabled(1< setMode(1); + I2Cdev::writeByte(0x0E, 0x0A, 0x01, wireObj); + + // setup AK8975 (0x0E) as Slave 0 in read mode + DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA, wireObj); + + // setup AK8975 (0x0E) as Slave 2 in write mode + DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01, wireObj); + + // setup I2C timing/delay control + DEBUG_PRINTLN(F("Setting up slave access delay...")); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18, wireObj); + I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05, wireObj); + + // enable interrupts + DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); + I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00, wireObj); + + // enable I2C master mode and reset DMP/FIFO + DEBUG_PRINTLN(F("Enabling I2C master mode...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20, wireObj); + DEBUG_PRINTLN(F("Resetting FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24, wireObj); + DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20, wireObj); + DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); + I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8, wireObj); + + DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + #ifdef DEBUG + DEBUG_PRINT(F("Read bytes: ")); + for (j = 0; j < 4; j++) { + DEBUG_PRINTF(dmpUpdate[3 + j], HEX); + DEBUG_PRINT(" "); + } + DEBUG_PRINTLN(""); + #endif + + DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); + while ((fifoCount = getFIFOCount()) < 46); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); + while ((fifoCount = getFIFOCount()) < 48); + DEBUG_PRINTLN(F("Reading FIFO...")); + getFIFOBytes(fifoBuffer, (fifoCount < 128) ? fifoCount : 128); // safeguard only 128 bytes + DEBUG_PRINTLN(F("Reading interrupt status...")); + getIntStatus(); + + DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); + for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); + writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); + + DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); + setDMPEnabled(false); + + DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); + dmpPacketSize = 48; + /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { + return 3; // TODO: proper error code for no memory + }*/ + + DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); + resetFIFO(); + getIntStatus(); + } else { + DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); + return 2; // configuration block loading failed + } + } else { + DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); + return 1; // main binary block loading failed + } + return 0; // success +} + +bool MPU6050_9Axis_MotionApps41::dmpPacketAvailable() { + return getFIFOCount() >= dmpGetFIFOPacketSize(); +} + +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetFIFORate(uint8_t fifoRate); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetFIFORate(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetSampleStepSizeMS(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetSampleFrequency(); +// int32_t MPU6050_9Axis_MotionApps41::dmpDecodeTemperature(int8_t tempReg); + +//uint8_t MPU6050_9Axis_MotionApps41::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); +//uint8_t MPU6050_9Axis_MotionApps41::dmpUnregisterFIFORateProcess(inv_obj_func func); +//uint8_t MPU6050_9Axis_MotionApps41::dmpRunFIFORateProcesses(); + +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendQuaternion(uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendPacketNumber(uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + +uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccel(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]); + data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]); + data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccel(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[34] << 8) | packet[35]; + data[1] = (packet[38] << 8) | packet[39]; + data[2] = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + v -> x = (packet[34] << 8) | packet[35]; + v -> y = (packet[38] << 8) | packet[39]; + v -> z = (packet[42] << 8) | packet[43]; + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); + data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); + data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); + data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = ((packet[0] << 8) | packet[1]); + data[1] = ((packet[4] << 8) | packet[5]); + data[2] = ((packet[8] << 8) | packet[9]); + data[3] = ((packet[12] << 8) | packet[13]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + if (status == 0) { + q -> w = (float)qI[0] / 16384.0f; + q -> x = (float)qI[1] / 16384.0f; + q -> y = (float)qI[2] / 16384.0f; + q -> z = (float)qI[3] / 16384.0f; + return 0; + } + return status; // int16 return value, indicates error if this line is reached +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyro(int32_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); + data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); + data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyro(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[16] << 8) | packet[17]; + data[1] = (packet[20] << 8) | packet[21]; + data[2] = (packet[24] << 8) | packet[25]; + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpGetMag(int16_t *data, const uint8_t* packet) { + // TODO: accommodate different arrangements of sent data (ONLY default supported now) + if (packet == 0) packet = dmpPacketBuffer; + data[0] = (packet[28] << 8) | packet[29]; + data[1] = (packet[30] << 8) | packet[31]; + data[2] = (packet[32] << 8) | packet[33]; + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetLinearAccelFilterCoefficient(float coef); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccel(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { + // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) + v -> x = vRaw -> x - gravity -> x*4096; + v -> y = vRaw -> y - gravity -> y*4096; + v -> z = vRaw -> z - gravity -> z*4096; + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { + // rotate measured 3D acceleration vector into original state + // frame of reference based on orientation quaternion + memcpy(v, vReal, sizeof(VectorInt16)); + v -> rotate(q); + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetGyroSensor(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetControlData(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetTemperature(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetGravity(long *data, const uint8_t* packet); +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGravity(int16_t *data, const uint8_t* packet) { + /* +1g corresponds to +8192, sensitivity is 2g. */ + int16_t qI[4]; + uint8_t status = dmpGetQuaternion(qI, packet); + data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; + data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; + data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] + - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (int32_t)(2 * 16384L); + return status; +} + +uint8_t MPU6050_9Axis_MotionApps41::dmpGetGravity(VectorFloat *v, Quaternion *q) { + v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); + v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); + v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; + return 0; +} +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuantizedAccel(long *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetEIS(long *data, const uint8_t* packet); + +uint8_t MPU6050_9Axis_MotionApps41::dmpGetEuler(float *data, Quaternion *q) { + 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 + data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta + 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 + return 0; +} + +#ifdef USE_OLD_DMPGETYAWPITCHROLL +uint8_t MPU6050_9Axis_MotionApps41::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); + return 0; +} +#else +uint8_t MPU6050_9Axis_MotionApps41::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { + // yaw: (about Z axis) + data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); + // pitch: (nose up/down, about Y axis) + data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); + // roll: (tilt left/right, about X axis) + data[2] = atan2(gravity -> y , gravity -> z); + if(gravity->z<0) { + if(data[1]>0) { + data[1] = PI - data[1]; + } else { + data[1] = -PI - data[1]; + } + } + return 0; +} +#endif + +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetAccelFloat(float *data, const uint8_t* packet); +// uint8_t MPU6050_9Axis_MotionApps41::dmpGetQuaternionFloat(float *data, const uint8_t* packet); + +uint8_t MPU6050_9Axis_MotionApps41::dmpProcessFIFOPacket(const unsigned char *dmpData) { + (void)dmpData; // suppress unused variable compiler warning + /*for (uint8_t k = 0; k < dmpPacketSize; k++) { + if (dmpData[k] < 0x10) Serial.print("0"); + Serial.print(dmpData[k], HEX); + Serial.print(" "); + } + Serial.print("\n");*/ + //Serial.println((uint16_t)dmpPacketBuffer); + return 0; +} +uint8_t MPU6050_9Axis_MotionApps41::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { + uint8_t status; + uint8_t buf[dmpPacketSize]; + for (uint8_t i = 0; i < numPackets; i++) { + // read packet from FIFO + getFIFOBytes(buf, dmpPacketSize); + + // process packet + if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; + + // increment external process count variable, if supplied + if (processed != 0) (*processed)++; + } + return 0; +} + +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetFIFOProcessedCallback(void (*func) (void)); + +// uint8_t MPU6050_9Axis_MotionApps41::dmpInitFIFOParam(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpCloseFIFO(); +// uint8_t MPU6050_9Axis_MotionApps41::dmpSetGyroDataSource(uint_fast8_t source); +// uint8_t MPU6050_9Axis_MotionApps41::dmpDecodeQuantizedAccel(); +// uint32_t MPU6050_9Axis_MotionApps41::dmpGetGyroSumOfSquare(); +// uint32_t MPU6050_9Axis_MotionApps41::dmpGetAccelSumOfSquare(); +// void MPU6050_9Axis_MotionApps41::dmpOverrideQuaternion(long *q); +uint16_t MPU6050_9Axis_MotionApps41::dmpGetFIFOPacketSize() { + return dmpPacketSize; +} diff --git a/src/MPU6050_9Axis_MotionApps41.h b/src/MPU6050_9Axis_MotionApps41.h old mode 100755 new mode 100644 index 3e2608b..6a65b16 --- a/src/MPU6050_9Axis_MotionApps41.h +++ b/src/MPU6050_9Axis_MotionApps41.h @@ -1,11 +1,16 @@ -// I2Cdev library collection - MPU6050 I2C device class, 9-axis MotionApps 4.1 implementation +// I2Cdev library collection - MPU6050 I2C device class // Based on InvenSense MPU-6050 register map document rev. 2.0, 5/19/2011 (RM-MPU-6000A-00) -// 6/18/2012 by Jeff Rowberg +// 10/3/2011 by Jeff Rowberg // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib // // Changelog: +// 2021/09/27 - split implementations out of header files, finally // ... - ongoing debug release +// NOTE: THIS IS ONLY A PARIAL RELEASE. THIS DEVICE CLASS IS CURRENTLY UNDERGOING ACTIVE +// DEVELOPMENT AND IS STILL MISSING SOME IMPORTANT FEATURES. PLEASE KEEP THIS IN MIND IF +// YOU DECIDE TO USE THIS PARTICULAR CODE FOR ANYTHING. + /* ============================================ I2Cdev device library code is placed under the MIT license Copyright (c) 2012 Jeff Rowberg @@ -30,860 +35,119 @@ THE SOFTWARE. =============================================== */ -#ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_ -#define _MPU6050_9AXIS_MOTIONAPPS41_H_ - -#include "I2Cdev.h" -#include "helper_3dmath.h" +#ifndef _MPU6050_6AXIS_MOTIONAPPS41_H_ +#define _MPU6050_6AXIS_MOTIONAPPS41_H_ -// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board -#define MPU6050_INCLUDE_DMP_MOTIONAPPS41 +// take ownership of the "MPU6050" typedef +#define I2CDEVLIB_MPU6050_TYPEDEF #include "MPU6050.h" -// Tom Carpenter's conditional PROGMEM code -// http://forum.arduino.cc/index.php?topic=129407.0 -#ifdef __AVR__ - #include -#elif defined(ESP32) - #include -#else - // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen - #ifndef __PGMSPACE_H_ - #define __PGMSPACE_H_ 1 - #include +class MPU6050_9Axis_MotionApps41 : public MPU6050_Base { + public: + MPU6050_9Axis_MotionApps41(uint8_t address=MPU6050_DEFAULT_ADDRESS, void *wireObj=0) : MPU6050_Base(address, wireObj) { } - #define PROGMEM - #define PGM_P const char * - #define PSTR(str) (str) - #define F(x) x + uint8_t dmpInitialize(uint8_t rateDivisor = MPU6050_DMP_FIFO_RATE_DIVISOR, uint8_t mpuAddr = 0x68); + bool dmpPacketAvailable(); - typedef void prog_void; - typedef char prog_char; - //typedef unsigned char prog_uchar; - typedef int8_t prog_int8_t; - typedef uint8_t prog_uint8_t; - typedef int16_t prog_int16_t; - typedef uint16_t prog_uint16_t; - typedef int32_t prog_int32_t; - typedef uint32_t prog_uint32_t; + uint8_t dmpSetFIFORate(uint8_t fifoRate); + uint8_t dmpGetFIFORate(); + uint8_t dmpGetSampleStepSizeMS(); + uint8_t dmpGetSampleFrequency(); + int32_t dmpDecodeTemperature(int8_t tempReg); - #define strcpy_P(dest, src) strcpy((dest), (src)) - #define strcat_P(dest, src) strcat((dest), (src)) - #define strcmp_P(a, b) strcmp((a), (b)) + // Register callbacks after a packet of FIFO data is processed + //uint8_t dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); + //uint8_t dmpUnregisterFIFORateProcess(inv_obj_func func); + uint8_t dmpRunFIFORateProcesses(); - #define pgm_read_byte(addr) (*(const unsigned char *)(addr)) - #define pgm_read_word(addr) (*(const unsigned short *)(addr)) - #define pgm_read_dword(addr) (*(const unsigned long *)(addr)) - #define pgm_read_float(addr) (*(const float *)(addr)) + // Setup FIFO for various output + uint8_t dmpSendQuaternion(uint_fast16_t accuracy); + uint8_t dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendPacketNumber(uint_fast16_t accuracy); + uint8_t dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); + uint8_t dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); + + // Get Fixed Point data from FIFO + uint8_t dmpGetAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGet6AxisQuaternion(Quaternion *q, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetRelativeQuaternion(Quaternion *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyro(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetMag(int16_t *data, const uint8_t* packet=0); + uint8_t dmpSetLinearAccelFilterCoefficient(float coef); + uint8_t dmpGetLinearAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity); + uint8_t dmpGetLinearAccelInWorld(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q); + uint8_t dmpGetGyroAndAccelSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroAndAccelSensor(VectorInt16 *g, VectorInt16 *a, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGyroSensor(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetControlData(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetTemperature(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetGravity(VectorFloat *v, Quaternion *q); + uint8_t dmpGetUnquantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetUnquantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int32_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(int16_t *data, const uint8_t* packet=0); + uint8_t dmpGetQuantizedAccel(VectorInt16 *v, const uint8_t* packet=0); + uint8_t dmpGetExternalSensorData(int32_t *data, uint16_t size, const uint8_t* packet=0); + uint8_t dmpGetEIS(int32_t *data, const uint8_t* packet=0); - #define pgm_read_byte_near(addr) pgm_read_byte(addr) - #define pgm_read_word_near(addr) pgm_read_word(addr) - #define pgm_read_dword_near(addr) pgm_read_dword(addr) - #define pgm_read_float_near(addr) pgm_read_float(addr) - #define pgm_read_byte_far(addr) pgm_read_byte(addr) - #define pgm_read_word_far(addr) pgm_read_word(addr) - #define pgm_read_dword_far(addr) pgm_read_dword(addr) - #define pgm_read_float_far(addr) pgm_read_float(addr) - #endif -#endif - -// NOTE! Enabling DEBUG adds about 3.3kB to the flash program size. -// Debug output is now working even on ATMega328P MCUs (e.g. Arduino Uno) -// after moving string constants to flash memory storage using the F() -// compiler macro (Arduino IDE 1.0+ required). - -//#define DEBUG -#ifdef DEBUG - #define DEBUG_PRINT(x) Serial.print(x) - #define DEBUG_PRINTF(x, y) Serial.print(x, y) - #define DEBUG_PRINTLN(x) Serial.println(x) - #define DEBUG_PRINTLNF(x, y) Serial.println(x, y) -#else - #define DEBUG_PRINT(x) - #define DEBUG_PRINTF(x, y) - #define DEBUG_PRINTLN(x) - #define DEBUG_PRINTLNF(x, y) -#endif - -#define MPU6050_DMP_CODE_SIZE 1962 // dmpMemory[] -#define MPU6050_DMP_CONFIG_SIZE 232 // dmpConfig[] -#define MPU6050_DMP_UPDATES_SIZE 140 // dmpUpdates[] - -/* ================================================================================================ * - | Default MotionApps v4.1 48-byte FIFO packet structure: | - | | - | [QUAT W][ ][QUAT X][ ][QUAT Y][ ][QUAT Z][ ][GYRO X][ ][GYRO Y][ ] | - | 0 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 | - | | - | [GYRO Z][ ][MAG X ][MAG Y ][MAG Z ][ACC X ][ ][ACC Y ][ ][ACC Z ][ ][ ] | - | 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 | - * ================================================================================================ */ - -// this block of memory gets written to the MPU on start-up, and it seems -// to be volatile memory, so it has to be done each time (it only takes ~1 -// second though) -const unsigned char dmpMemory[MPU6050_DMP_CODE_SIZE] PROGMEM = { - // bank 0, 256 bytes - 0xFB, 0x00, 0x00, 0x3E, 0x00, 0x0B, 0x00, 0x36, 0x00, 0x01, 0x00, 0x02, 0x00, 0x03, 0x00, 0x00, - 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0xFA, 0x80, 0x00, 0x0B, 0x12, 0x82, 0x00, 0x01, - 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x28, 0x00, 0x00, 0xFF, 0xFF, 0x45, 0x81, 0xFF, 0xFF, 0xFA, 0x72, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x03, 0xE8, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x7F, 0xFF, 0xFF, 0xFE, 0x80, 0x01, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x3E, 0x03, 0x30, 0x40, 0x00, 0x00, 0x00, 0x02, 0xCA, 0xE3, 0x09, 0x3E, 0x80, 0x00, 0x00, - 0x20, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x60, 0x00, 0x00, 0x00, - 0x41, 0xFF, 0x00, 0x00, 0x00, 0x00, 0x0B, 0x2A, 0x00, 0x00, 0x16, 0x55, 0x00, 0x00, 0x21, 0x82, - 0xFD, 0x87, 0x26, 0x50, 0xFD, 0x80, 0x00, 0x00, 0x00, 0x1F, 0x00, 0x00, 0x00, 0x05, 0x80, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x03, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x04, 0x6F, 0x00, 0x02, 0x65, 0x32, 0x00, 0x00, 0x5E, 0xC0, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0xFB, 0x8C, 0x6F, 0x5D, 0xFD, 0x5D, 0x08, 0xD9, 0x00, 0x7C, 0x73, 0x3B, 0x00, 0x6C, 0x12, 0xCC, - 0x32, 0x00, 0x13, 0x9D, 0x32, 0x00, 0xD0, 0xD6, 0x32, 0x00, 0x08, 0x00, 0x40, 0x00, 0x01, 0xF4, - 0xFF, 0xE6, 0x80, 0x79, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0xD0, 0xD6, 0x00, 0x00, 0x27, 0x10, - - // bank 1, 256 bytes - 0xFB, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x01, 0x00, 0x01, 0x00, 0x00, 0x00, - 0x00, 0x00, 0xFA, 0x36, 0xFF, 0xBC, 0x30, 0x8E, 0x00, 0x05, 0xFB, 0xF0, 0xFF, 0xD9, 0x5B, 0xC8, - 0xFF, 0xD0, 0x9A, 0xBE, 0x00, 0x00, 0x10, 0xA9, 0xFF, 0xF4, 0x1E, 0xB2, 0x00, 0xCE, 0xBB, 0xF7, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x04, 0x00, 0x02, 0x00, 0x02, 0x02, 0x00, 0x00, 0x0C, - 0xFF, 0xC2, 0x80, 0x00, 0x00, 0x01, 0x80, 0x00, 0x00, 0xCF, 0x80, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x06, 0x00, 0x00, 0x00, 0x00, 0x14, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x03, 0x3F, 0x68, 0xB6, 0x79, 0x35, 0x28, 0xBC, 0xC6, 0x7E, 0xD1, 0x6C, - 0x80, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0xB2, 0x6A, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x3F, 0xF0, 0x00, 0x00, 0x00, 0x30, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x25, 0x4D, 0x00, 0x2F, 0x70, 0x6D, 0x00, 0x00, 0x05, 0xAE, 0x00, 0x0C, 0x02, 0xD0, - - // bank 2, 256 bytes - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x54, 0xFF, 0xEF, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x01, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x0C, 0x00, 0x00, 0x00, 0x01, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x65, 0x00, 0x00, 0x00, 0x54, 0x00, 0x00, 0xFF, 0xEF, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x40, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x01, 0x00, 0x00, 0x00, 0x02, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x1B, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x47, 0x78, 0xA2, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - // bank 3, 256 bytes - 0xD8, 0xDC, 0xF4, 0xD8, 0xB9, 0xAB, 0xF3, 0xF8, 0xFA, 0xF1, 0xBA, 0xA2, 0xDE, 0xB2, 0xB8, 0xB4, - 0xA8, 0x81, 0x98, 0xF7, 0x4A, 0x90, 0x7F, 0x91, 0x6A, 0xF3, 0xF9, 0xDB, 0xA8, 0xF9, 0xB0, 0xBA, - 0xA0, 0x80, 0xF2, 0xCE, 0x81, 0xF3, 0xC2, 0xF1, 0xC1, 0xF2, 0xC3, 0xF3, 0xCC, 0xA2, 0xB2, 0x80, - 0xF1, 0xC6, 0xD8, 0x80, 0xBA, 0xA7, 0xDF, 0xDF, 0xDF, 0xF2, 0xA7, 0xC3, 0xCB, 0xC5, 0xB6, 0xF0, - 0x87, 0xA2, 0x94, 0x24, 0x48, 0x70, 0x3C, 0x95, 0x40, 0x68, 0x34, 0x58, 0x9B, 0x78, 0xA2, 0xF1, - 0x83, 0x92, 0x2D, 0x55, 0x7D, 0xD8, 0xB1, 0xB4, 0xB8, 0xA1, 0xD0, 0x91, 0x80, 0xF2, 0x70, 0xF3, - 0x70, 0xF2, 0x7C, 0x80, 0xA8, 0xF1, 0x01, 0xB0, 0x98, 0x87, 0xD9, 0x43, 0xD8, 0x86, 0xC9, 0x88, - 0xBA, 0xA1, 0xF2, 0x0E, 0xB8, 0x97, 0x80, 0xF1, 0xA9, 0xDF, 0xDF, 0xDF, 0xAA, 0xDF, 0xDF, 0xDF, - 0xF2, 0xAA, 0xC5, 0xCD, 0xC7, 0xA9, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, 0x97, 0xF1, 0xA9, 0x89, - 0x26, 0x46, 0x66, 0xB0, 0xB4, 0xBA, 0x80, 0xAC, 0xDE, 0xF2, 0xCA, 0xF1, 0xB2, 0x8C, 0x02, 0xA9, - 0xB6, 0x98, 0x00, 0x89, 0x0E, 0x16, 0x1E, 0xB8, 0xA9, 0xB4, 0x99, 0x2C, 0x54, 0x7C, 0xB0, 0x8A, - 0xA8, 0x96, 0x36, 0x56, 0x76, 0xF1, 0xB9, 0xAF, 0xB4, 0xB0, 0x83, 0xC0, 0xB8, 0xA8, 0x97, 0x11, - 0xB1, 0x8F, 0x98, 0xB9, 0xAF, 0xF0, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xF1, 0xA3, 0x29, 0x55, - 0x7D, 0xAF, 0x83, 0xB5, 0x93, 0xF0, 0x00, 0x28, 0x50, 0xF5, 0xBA, 0xAD, 0x8F, 0x9F, 0x28, 0x54, - 0x7C, 0xB9, 0xF1, 0xA3, 0x86, 0x9F, 0x61, 0xA6, 0xDA, 0xDE, 0xDF, 0xDB, 0xB2, 0xB6, 0x8E, 0x9D, - 0xAE, 0xF5, 0x60, 0x68, 0x70, 0xB1, 0xB5, 0xF1, 0xDA, 0xA6, 0xDF, 0xD9, 0xA6, 0xFA, 0xA3, 0x86, - - // bank 4, 256 bytes - 0x96, 0xDB, 0x31, 0xA6, 0xD9, 0xF8, 0xDF, 0xBA, 0xA6, 0x8F, 0xC2, 0xC5, 0xC7, 0xB2, 0x8C, 0xC1, - 0xB8, 0xA2, 0xDF, 0xDF, 0xDF, 0xA3, 0xDF, 0xDF, 0xDF, 0xD8, 0xD8, 0xF1, 0xB8, 0xA8, 0xB2, 0x86, - 0xB4, 0x98, 0x0D, 0x35, 0x5D, 0xB8, 0xAA, 0x98, 0xB0, 0x87, 0x2D, 0x35, 0x3D, 0xB2, 0xB6, 0xBA, - 0xAF, 0x8C, 0x96, 0x19, 0x8F, 0x9F, 0xA7, 0x0E, 0x16, 0x1E, 0xB4, 0x9A, 0xB8, 0xAA, 0x87, 0x2C, - 0x54, 0x7C, 0xB9, 0xA3, 0xDE, 0xDF, 0xDF, 0xA3, 0xB1, 0x80, 0xF2, 0xC4, 0xCD, 0xC9, 0xF1, 0xB8, - 0xA9, 0xB4, 0x99, 0x83, 0x0D, 0x35, 0x5D, 0x89, 0xB9, 0xA3, 0x2D, 0x55, 0x7D, 0xB5, 0x93, 0xA3, - 0x0E, 0x16, 0x1E, 0xA9, 0x2C, 0x54, 0x7C, 0xB8, 0xB4, 0xB0, 0xF1, 0x97, 0x83, 0xA8, 0x11, 0x84, - 0xA5, 0x09, 0x98, 0xA3, 0x83, 0xF0, 0xDA, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xD8, 0xF1, 0xA5, - 0x29, 0x55, 0x7D, 0xA5, 0x85, 0x95, 0x02, 0x1A, 0x2E, 0x3A, 0x56, 0x5A, 0x40, 0x48, 0xF9, 0xF3, - 0xA3, 0xD9, 0xF8, 0xF0, 0x98, 0x83, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0x97, 0x82, 0xA8, 0xF1, - 0x11, 0xF0, 0x98, 0xA2, 0x24, 0x08, 0x44, 0x10, 0x64, 0x18, 0xDA, 0xF3, 0xDE, 0xD8, 0x83, 0xA5, - 0x94, 0x01, 0xD9, 0xA3, 0x02, 0xF1, 0xA2, 0xC3, 0xC5, 0xC7, 0xD8, 0xF1, 0x84, 0x92, 0xA2, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0x93, 0xA3, 0x4D, - 0xDA, 0x2A, 0xD8, 0x48, 0x69, 0xD9, 0x2A, 0xD8, 0x68, 0x55, 0xDA, 0x32, 0xD8, 0x50, 0x71, 0xD9, - 0x32, 0xD8, 0x70, 0x5D, 0xDA, 0x3A, 0xD8, 0x58, 0x79, 0xD9, 0x3A, 0xD8, 0x78, 0xA8, 0x8A, 0x9A, - - // bank 5, 256 bytes - 0xF0, 0x28, 0x50, 0x78, 0x9E, 0xF3, 0x88, 0x18, 0xF1, 0x9F, 0x1D, 0x98, 0xA8, 0xD9, 0x08, 0xD8, - 0xC8, 0x9F, 0x12, 0x9E, 0xF3, 0x15, 0xA8, 0xDA, 0x12, 0x10, 0xD8, 0xF1, 0xAF, 0xC8, 0x97, 0x87, - 0x34, 0xB5, 0xB9, 0x94, 0xA4, 0x21, 0xF3, 0xD9, 0x22, 0xD8, 0xF2, 0x2D, 0xF3, 0xD9, 0x2A, 0xD8, - 0xF2, 0x35, 0xF3, 0xD9, 0x32, 0xD8, 0x81, 0xA4, 0x60, 0x60, 0x61, 0xD9, 0x61, 0xD8, 0x6C, 0x68, - 0x69, 0xD9, 0x69, 0xD8, 0x74, 0x70, 0x71, 0xD9, 0x71, 0xD8, 0xB1, 0xA3, 0x84, 0x19, 0x3D, 0x5D, - 0xA3, 0x83, 0x1A, 0x3E, 0x5E, 0x93, 0x10, 0x30, 0x81, 0x10, 0x11, 0xB8, 0xB0, 0xAF, 0x8F, 0x94, - 0xF2, 0xDA, 0x3E, 0xD8, 0xB4, 0x9A, 0xA8, 0x87, 0x29, 0xDA, 0xF8, 0xD8, 0x87, 0x9A, 0x35, 0xDA, - 0xF8, 0xD8, 0x87, 0x9A, 0x3D, 0xDA, 0xF8, 0xD8, 0xB1, 0xB9, 0xA4, 0x98, 0x85, 0x02, 0x2E, 0x56, - 0xA5, 0x81, 0x00, 0x0C, 0x14, 0xA3, 0x97, 0xB0, 0x8A, 0xF1, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, 0xD9, - 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x84, 0x0D, 0xDA, 0x0E, 0xD8, 0xA3, 0x29, 0x83, 0xDA, - 0x2C, 0x0E, 0xD8, 0xA3, 0x84, 0x49, 0x83, 0xDA, 0x2C, 0x4C, 0x0E, 0xD8, 0xB8, 0xB0, 0x97, 0x86, - 0xA8, 0x31, 0x9B, 0x06, 0x99, 0x07, 0xAB, 0x97, 0x28, 0x88, 0x9B, 0xF0, 0x0C, 0x20, 0x14, 0x40, - 0xB9, 0xA3, 0x8A, 0xC3, 0xC5, 0xC7, 0x9A, 0xA3, 0x28, 0x50, 0x78, 0xF1, 0xB5, 0x93, 0x01, 0xD9, - 0xDF, 0xDF, 0xDF, 0xD8, 0xB8, 0xB4, 0xA8, 0x8C, 0x9C, 0xF0, 0x04, 0x28, 0x51, 0x79, 0x1D, 0x30, - 0x14, 0x38, 0xB2, 0x82, 0xAB, 0xD0, 0x98, 0x2C, 0x50, 0x50, 0x78, 0x78, 0x9B, 0xF1, 0x1A, 0xB0, - 0xF0, 0xB1, 0x83, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0xB0, 0x8B, 0x29, 0x51, 0x79, 0xB1, 0x83, 0x24, - - // bank 6, 256 bytes - 0x70, 0x59, 0xB0, 0x8B, 0x20, 0x58, 0x71, 0xB1, 0x83, 0x44, 0x69, 0x38, 0xB0, 0x8B, 0x39, 0x40, - 0x68, 0xB1, 0x83, 0x64, 0x48, 0x31, 0xB0, 0x8B, 0x30, 0x49, 0x60, 0xA5, 0x88, 0x20, 0x09, 0x71, - 0x58, 0x44, 0x68, 0x11, 0x39, 0x64, 0x49, 0x30, 0x19, 0xF1, 0xAC, 0x00, 0x2C, 0x54, 0x7C, 0xF0, - 0x8C, 0xA8, 0x04, 0x28, 0x50, 0x78, 0xF1, 0x88, 0x97, 0x26, 0xA8, 0x59, 0x98, 0xAC, 0x8C, 0x02, - 0x26, 0x46, 0x66, 0xF0, 0x89, 0x9C, 0xA8, 0x29, 0x51, 0x79, 0x24, 0x70, 0x59, 0x44, 0x69, 0x38, - 0x64, 0x48, 0x31, 0xA9, 0x88, 0x09, 0x20, 0x59, 0x70, 0xAB, 0x11, 0x38, 0x40, 0x69, 0xA8, 0x19, - 0x31, 0x48, 0x60, 0x8C, 0xA8, 0x3C, 0x41, 0x5C, 0x20, 0x7C, 0x00, 0xF1, 0x87, 0x98, 0x19, 0x86, - 0xA8, 0x6E, 0x76, 0x7E, 0xA9, 0x99, 0x88, 0x2D, 0x55, 0x7D, 0x9E, 0xB9, 0xA3, 0x8A, 0x22, 0x8A, - 0x6E, 0x8A, 0x56, 0x8A, 0x5E, 0x9F, 0xB1, 0x83, 0x06, 0x26, 0x46, 0x66, 0x0E, 0x2E, 0x4E, 0x6E, - 0x9D, 0xB8, 0xAD, 0x00, 0x2C, 0x54, 0x7C, 0xF2, 0xB1, 0x8C, 0xB4, 0x99, 0xB9, 0xA3, 0x2D, 0x55, - 0x7D, 0x81, 0x91, 0xAC, 0x38, 0xAD, 0x3A, 0xB5, 0x83, 0x91, 0xAC, 0x2D, 0xD9, 0x28, 0xD8, 0x4D, - 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0x8C, 0x9D, 0xAE, 0x29, 0xD9, 0x04, 0xAE, 0xD8, 0x51, - 0xD9, 0x04, 0xAE, 0xD8, 0x79, 0xD9, 0x04, 0xD8, 0x81, 0xF3, 0x9D, 0xAD, 0x00, 0x8D, 0xAE, 0x19, - 0x81, 0xAD, 0xD9, 0x01, 0xD8, 0xF2, 0xAE, 0xDA, 0x26, 0xD8, 0x8E, 0x91, 0x29, 0x83, 0xA7, 0xD9, - 0xAD, 0xAD, 0xAD, 0xAD, 0xF3, 0x2A, 0xD8, 0xD8, 0xF1, 0xB0, 0xAC, 0x89, 0x91, 0x3E, 0x5E, 0x76, - 0xF3, 0xAC, 0x2E, 0x2E, 0xF1, 0xB1, 0x8C, 0x5A, 0x9C, 0xAC, 0x2C, 0x28, 0x28, 0x28, 0x9C, 0xAC, - - // bank 7, 170 bytes (remainder) - 0x30, 0x18, 0xA8, 0x98, 0x81, 0x28, 0x34, 0x3C, 0x97, 0x24, 0xA7, 0x28, 0x34, 0x3C, 0x9C, 0x24, - 0xF2, 0xB0, 0x89, 0xAC, 0x91, 0x2C, 0x4C, 0x6C, 0x8A, 0x9B, 0x2D, 0xD9, 0xD8, 0xD8, 0x51, 0xD9, - 0xD8, 0xD8, 0x79, 0xD9, 0xD8, 0xD8, 0xF1, 0x9E, 0x88, 0xA3, 0x31, 0xDA, 0xD8, 0xD8, 0x91, 0x2D, - 0xD9, 0x28, 0xD8, 0x4D, 0xD9, 0x48, 0xD8, 0x6D, 0xD9, 0x68, 0xD8, 0xB1, 0x83, 0x93, 0x35, 0x3D, - 0x80, 0x25, 0xDA, 0xD8, 0xD8, 0x85, 0x69, 0xDA, 0xD8, 0xD8, 0xB4, 0x93, 0x81, 0xA3, 0x28, 0x34, - 0x3C, 0xF3, 0xAB, 0x8B, 0xA3, 0x91, 0xB6, 0x09, 0xB4, 0xD9, 0xAB, 0xDE, 0xB0, 0x87, 0x9C, 0xB9, - 0xA3, 0xDD, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x95, 0xF1, 0xA3, 0xA3, 0xA3, 0x9D, 0xF1, 0xA3, 0xA3, - 0xA3, 0xA3, 0xF2, 0xA3, 0xB4, 0x90, 0x80, 0xF2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, - 0xA3, 0xA3, 0xB2, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xA3, 0xB0, 0x87, 0xB5, 0x99, 0xF1, 0xA3, 0xA3, - 0xA3, 0x98, 0xF1, 0xA3, 0xA3, 0xA3, 0xA3, 0x97, 0xA3, 0xA3, 0xA3, 0xA3, 0xF3, 0x9B, 0xA3, 0xA3, - 0xDC, 0xB9, 0xA7, 0xF1, 0x26, 0x26, 0x26, 0xD8, 0xD8, 0xFF + uint8_t dmpGetEuler(float *data, Quaternion *q); + uint8_t dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity); + + // Get Floating Point data from FIFO + uint8_t dmpGetAccelFloat(float *data, const uint8_t* packet=0); + uint8_t dmpGetQuaternionFloat(float *data, const uint8_t* packet=0); + + uint8_t dmpProcessFIFOPacket(const unsigned char *dmpData); + uint8_t dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed=NULL); + + uint8_t dmpSetFIFOProcessedCallback(void (*func) (void)); + + uint8_t dmpInitFIFOParam(); + uint8_t dmpCloseFIFO(); + uint8_t dmpSetGyroDataSource(uint8_t source); + uint8_t dmpDecodeQuantizedAccel(); + uint32_t dmpGetGyroSumOfSquare(); + uint32_t dmpGetAccelSumOfSquare(); + void dmpOverrideQuaternion(long *q); + uint16_t dmpGetFIFOPacketSize(); + + private: + uint8_t *dmpPacketBuffer; + uint16_t dmpPacketSize; }; -#ifndef MPU6050_DMP_FIFO_RATE_DIVISOR -#define MPU6050_DMP_FIFO_RATE_DIVISOR 0x03 -#endif - -const unsigned char dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { -// BANK OFFSET LENGTH [DATA] - 0x02, 0xEC, 0x04, 0x00, 0x47, 0x7D, 0x1A, // ? - 0x03, 0x82, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration - 0x03, 0xB2, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration - 0x00, 0x68, 0x04, 0x02, 0xCA, 0xE3, 0x09, // D_0_104 inv_set_gyro_calibration - 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration - 0x03, 0x86, 0x03, 0x0C, 0xC9, 0x2C, // FCFG_2 inv_set_accel_calibration - 0x03, 0x90, 0x03, 0x26, 0x46, 0x66, // (continued)...FCFG_2 inv_set_accel_calibration - 0x00, 0x6C, 0x02, 0x40, 0x00, // D_0_108 inv_set_accel_calibration - - 0x02, 0x40, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_00 inv_set_compass_calibration - 0x02, 0x44, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_01 - 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 - 0x02, 0x4C, 0x04, 0x40, 0x00, 0x00, 0x00, // CPASS_MTX_10 - 0x02, 0x50, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_11 - 0x02, 0x54, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_12 - 0x02, 0x58, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_20 - 0x02, 0x5C, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_21 - 0x02, 0xBC, 0x04, 0xC0, 0x00, 0x00, 0x00, // CPASS_MTX_22 - - 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel - 0x03, 0x86, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors - 0x04, 0x22, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion - 0x00, 0xA3, 0x01, 0x00, // ? - 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion - 0x07, 0x9F, 0x01, 0x30, // CFG_16 inv_set_footer - 0x07, 0x67, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro - 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? - 0x02, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // ? - 0x07, 0x83, 0x06, 0xC2, 0xCA, 0xC4, 0xA3, 0xA3, 0xA3, // ? - // SPECIAL 0x01 = enable interrupts - 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE, SPECIAL INSTRUCTION - 0x07, 0xA7, 0x01, 0xFE, // ? - 0x07, 0x62, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // ? - 0x07, 0x67, 0x01, 0x9A, // ? - 0x07, 0x68, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo - 0x07, 0x8D, 0x04, 0xF1, 0x28, 0x30, 0x38, // ??? CFG_12 inv_send_mag -> inv_construct3_fifo - 0x02, 0x16, 0x02, 0x00, MPU6050_DMP_FIFO_RATE_DIVISOR // D_0_22 inv_set_fifo_rate - - // This very last 0x03 WAS a 0x09, which drops the FIFO rate down to 20 Hz. 0x07 is 25 Hz, - // 0x01 is 100Hz. Going faster than 100Hz (0x00=200Hz) tends to result in very noisy data. - // DMP output frequency is calculated easily using this equation: (200Hz / (1 + value)) - - // It is important to make sure the host processor can keep up with reading and processing - // the FIFO output at the desired rate. Handling FIFO overflow cleanly is also a good idea. -}; - -const unsigned char dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { - 0x01, 0xB2, 0x02, 0xFF, 0xF5, - 0x01, 0x90, 0x04, 0x0A, 0x0D, 0x97, 0xC0, - 0x00, 0xA3, 0x01, 0x00, - 0x04, 0x29, 0x04, 0x87, 0x2D, 0x35, 0x3D, - 0x01, 0x6A, 0x02, 0x06, 0x00, - 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x02, 0x60, 0x0C, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x01, 0x08, 0x02, 0x01, 0x20, - 0x01, 0x0A, 0x02, 0x00, 0x4E, - 0x01, 0x02, 0x02, 0xFE, 0xB3, - 0x02, 0x6C, 0x04, 0x00, 0x00, 0x00, 0x00, // READ - 0x02, 0x6C, 0x04, 0xFA, 0xFE, 0x00, 0x00, - 0x02, 0x60, 0x0C, 0xFF, 0xFF, 0xCB, 0x4D, 0x00, 0x01, 0x08, 0xC1, 0xFF, 0xFF, 0xBC, 0x2C, - 0x02, 0xF4, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x02, 0xF8, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x02, 0xFC, 0x04, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 -}; - -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - reset(); - delay(30); // wait after reset - - // disable sleep mode - DEBUG_PRINTLN(F("Disabling sleep mode...")); - setSleepEnabled(false); - - // get MPU product ID - DEBUG_PRINTLN(F("Getting product ID...")); - //uint8_t productID = 0; //getProductID(); - DEBUG_PRINT(F("Product ID = ")); - DEBUG_PRINT(productID); - - // get MPU hardware revision - DEBUG_PRINTLN(F("Selecting user bank 16...")); - setMemoryBank(0x10, true, true); - DEBUG_PRINTLN(F("Selecting memory byte 6...")); - setMemoryStartAddress(0x06); - DEBUG_PRINTLN(F("Checking hardware revision...")); - uint8_t hwRevision = readMemoryByte(); - DEBUG_PRINT(F("Revision @ user[16][6] = ")); - DEBUG_PRINTLNF(hwRevision, HEX); - DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); - setMemoryBank(0, false, false); - - // check OTP bank valid - DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); - uint8_t otpValid = getOTPBankValid(); - DEBUG_PRINT(F("OTP bank is ")); - DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); - - // get X/Y/Z gyro offsets - DEBUG_PRINTLN(F("Reading gyro offset values...")); - int8_t xgOffset = getXGyroOffset(); - int8_t ygOffset = getYGyroOffset(); - int8_t zgOffset = getZGyroOffset(); - DEBUG_PRINT(F("X gyro offset = ")); - DEBUG_PRINTLN(xgOffset); - DEBUG_PRINT(F("Y gyro offset = ")); - DEBUG_PRINTLN(ygOffset); - DEBUG_PRINT(F("Z gyro offset = ")); - DEBUG_PRINTLN(zgOffset); - - I2Cdev::readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ? - - DEBUG_PRINTLN(F("Enabling interrupt latch, clear on any read, AUX bypass enabled")); - I2Cdev::writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32); - - // enable MPU AUX I2C bypass mode - //DEBUG_PRINTLN(F("Enabling AUX I2C bypass mode...")); - //setI2CBypassEnabled(true); - - DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); - //mag -> setMode(0); - I2Cdev::writeByte(0x0E, 0x0A, 0x00); - - DEBUG_PRINTLN(F("Setting magnetometer mode to fuse access...")); - //mag -> setMode(0x0F); - I2Cdev::writeByte(0x0E, 0x0A, 0x0F); - - DEBUG_PRINTLN(F("Reading mag magnetometer factory calibration...")); - int8_t asax, asay, asaz; - //mag -> getAdjustment(&asax, &asay, &asaz); - I2Cdev::readBytes(0x0E, 0x10, 3, buffer); - asax = (int8_t)buffer[0]; - asay = (int8_t)buffer[1]; - asaz = (int8_t)buffer[2]; - DEBUG_PRINT(F("Adjustment X/Y/Z = ")); - DEBUG_PRINT(asax); - DEBUG_PRINT(F(" / ")); - DEBUG_PRINT(asay); - DEBUG_PRINT(F(" / ")); - DEBUG_PRINTLN(asaz); - - DEBUG_PRINTLN(F("Setting magnetometer mode to power-down...")); - //mag -> setMode(0); - I2Cdev::writeByte(0x0E, 0x0A, 0x00); - - // load DMP code into memory banks - DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP code written and verified.")); - - DEBUG_PRINTLN(F("Configuring DMP and related settings...")); - - // write DMP configuration - DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); - DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); - DEBUG_PRINTLN(F(" bytes in config def)")); - if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - setIntEnabled(1< setMode(1); - I2Cdev::writeByte(0x0E, 0x0A, 0x01); - - // setup AK8975 (0x0E) as Slave 0 in read mode - DEBUG_PRINTLN(F("Setting up AK8975 read slave 0...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_ADDR, 0x8E); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_REG, 0x01); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); - - // setup AK8975 (0x0E) as Slave 2 in write mode - DEBUG_PRINTLN(F("Setting up AK8975 write slave 2...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_ADDR, 0x0E); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_REG, 0x0A); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_CTRL, 0x81); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV2_DO, 0x01); - - // setup I2C timing/delay control - DEBUG_PRINTLN(F("Setting up slave access delay...")); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18); - I2Cdev::writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); - - // enable interrupts - DEBUG_PRINTLN(F("Enabling default interrupt behavior/no bypass...")); - I2Cdev::writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00); - - // enable I2C master mode and reset DMP/FIFO - DEBUG_PRINTLN(F("Enabling I2C master mode...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); - DEBUG_PRINTLN(F("Resetting FIFO...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24); - DEBUG_PRINTLN(F("Rewriting I2C master mode enabled because...I don't know")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20); - DEBUG_PRINTLN(F("Enabling and resetting DMP/FIFO...")); - I2Cdev::writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8); - - DEBUG_PRINTLN(F("Writing final memory update 5/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 6/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 7/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 8/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 9/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 10/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 11/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Reading final memory update 12/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - #ifdef DEBUG - DEBUG_PRINT(F("Read bytes: ")); - for (j = 0; j < 4; j++) { - DEBUG_PRINTF(dmpUpdate[3 + j], HEX); - DEBUG_PRINT(" "); - } - DEBUG_PRINTLN(""); - #endif - - DEBUG_PRINTLN(F("Writing final memory update 13/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 14/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 15/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 16/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - DEBUG_PRINTLN(F("Writing final memory update 17/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIRO count >= 46...")); - while ((fifoCount = getFIFOCount()) < 46); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - - DEBUG_PRINTLN(F("Writing final memory update 18/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); - while ((fifoCount = getFIFOCount()) < 48); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - DEBUG_PRINTLN(F("Waiting for FIRO count >= 48...")); - while ((fifoCount = getFIFOCount()) < 48); - DEBUG_PRINTLN(F("Reading FIFO...")); - getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes - DEBUG_PRINTLN(F("Reading interrupt status...")); - getIntStatus(); - - DEBUG_PRINTLN(F("Writing final memory update 19/19 (function unknown)...")); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); - setDMPEnabled(false); - - DEBUG_PRINTLN(F("Setting up internal 48-byte (default) DMP packet buffer...")); - dmpPacketSize = 48; - /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { - return 3; // TODO: proper error code for no memory - }*/ - - DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); - resetFIFO(); - getIntStatus(); - } else { - DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); - return 2; // configuration block loading failed - } - } else { - DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); - return 1; // main binary block loading failed - } - return 0; // success -} - -bool MPU6050::dmpPacketAvailable() { - return getFIFOCount() >= dmpGetFIFOPacketSize(); -} - -// uint8_t MPU6050::dmpSetFIFORate(uint8_t fifoRate); -// uint8_t MPU6050::dmpGetFIFORate(); -// uint8_t MPU6050::dmpGetSampleStepSizeMS(); -// uint8_t MPU6050::dmpGetSampleFrequency(); -// int32_t MPU6050::dmpDecodeTemperature(int8_t tempReg); - -//uint8_t MPU6050::dmpRegisterFIFORateProcess(inv_obj_func func, int16_t priority); -//uint8_t MPU6050::dmpUnregisterFIFORateProcess(inv_obj_func func); -//uint8_t MPU6050::dmpRunFIFORateProcesses(); - -// uint8_t MPU6050::dmpSendQuaternion(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGyro(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendLinearAccelInWorld(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendControlData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendExternalSensorData(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendGravity(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendPacketNumber(uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendQuantizedAccel(uint_fast16_t elements, uint_fast16_t accuracy); -// uint8_t MPU6050::dmpSendEIS(uint_fast16_t elements, uint_fast16_t accuracy); - -uint8_t MPU6050::dmpGetAccel(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[34] << 24) | ((uint32_t)packet[35] << 16) | ((uint32_t)packet[36] << 8) | packet[37]); - data[1] = (((uint32_t)packet[38] << 24) | ((uint32_t)packet[39] << 16) | ((uint32_t)packet[40] << 8) | packet[41]); - data[2] = (((uint32_t)packet[42] << 24) | ((uint32_t)packet[43] << 16) | ((uint32_t)packet[44] << 8) | packet[45]); - return 0; -} -uint8_t MPU6050::dmpGetAccel(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[34] << 8) | packet[35]; - data[1] = (packet[38] << 8) | packet[39]; - data[2] = (packet[42] << 8) | packet[43]; - return 0; -} -uint8_t MPU6050::dmpGetAccel(VectorInt16 *v, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - v -> x = (packet[34] << 8) | packet[35]; - v -> y = (packet[38] << 8) | packet[39]; - v -> z = (packet[42] << 8) | packet[43]; - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[0] << 24) | ((uint32_t)packet[1] << 16) | ((uint32_t)packet[2] << 8) | packet[3]); - data[1] = (((uint32_t)packet[4] << 24) | ((uint32_t)packet[5] << 16) | ((uint32_t)packet[6] << 8) | packet[7]); - data[2] = (((uint32_t)packet[8] << 24) | ((uint32_t)packet[9] << 16) | ((uint32_t)packet[10] << 8) | packet[11]); - data[3] = (((uint32_t)packet[12] << 24) | ((uint32_t)packet[13] << 16) | ((uint32_t)packet[14] << 8) | packet[15]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = ((packet[0] << 8) | packet[1]); - data[1] = ((packet[4] << 8) | packet[5]); - data[2] = ((packet[8] << 8) | packet[9]); - data[3] = ((packet[12] << 8) | packet[13]); - return 0; -} -uint8_t MPU6050::dmpGetQuaternion(Quaternion *q, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - if (status == 0) { - q -> w = (float)qI[0] / 16384.0f; - q -> x = (float)qI[1] / 16384.0f; - q -> y = (float)qI[2] / 16384.0f; - q -> z = (float)qI[3] / 16384.0f; - return 0; - } - return status; // int16 return value, indicates error if this line is reached -} -// uint8_t MPU6050::dmpGet6AxisQuaternion(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetRelativeQuaternion(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGyro(int32_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (((uint32_t)packet[16] << 24) | ((uint32_t)packet[17] << 16) | ((uint32_t)packet[18] << 8) | packet[19]); - data[1] = (((uint32_t)packet[20] << 24) | ((uint32_t)packet[21] << 16) | ((uint32_t)packet[22] << 8) | packet[23]); - data[2] = (((uint32_t)packet[24] << 24) | ((uint32_t)packet[25] << 16) | ((uint32_t)packet[26] << 8) | packet[27]); - return 0; -} -uint8_t MPU6050::dmpGetGyro(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[16] << 8) | packet[17]; - data[1] = (packet[20] << 8) | packet[21]; - data[2] = (packet[24] << 8) | packet[25]; - return 0; -} -uint8_t MPU6050::dmpGetMag(int16_t *data, const uint8_t* packet) { - // TODO: accommodate different arrangements of sent data (ONLY default supported now) - if (packet == 0) packet = dmpPacketBuffer; - data[0] = (packet[28] << 8) | packet[29]; - data[1] = (packet[30] << 8) | packet[31]; - data[2] = (packet[32] << 8) | packet[33]; - return 0; -} -// uint8_t MPU6050::dmpSetLinearAccelFilterCoefficient(float coef); -// uint8_t MPU6050::dmpGetLinearAccel(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccel(VectorInt16 *v, VectorInt16 *vRaw, VectorFloat *gravity) { - // get rid of the gravity component (+1g = +4096 in standard DMP FIFO packet) - v -> x = vRaw -> x - gravity -> x*4096; - v -> y = vRaw -> y - gravity -> y*4096; - v -> z = vRaw -> z - gravity -> z*4096; - return 0; -} -// uint8_t MPU6050::dmpGetLinearAccelInWorld(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetLinearAccelInWorld(VectorInt16 *v, VectorInt16 *vReal, Quaternion *q) { - // rotate measured 3D acceleration vector into original state - // frame of reference based on orientation quaternion - memcpy(v, vReal, sizeof(VectorInt16)); - v -> rotate(q); - return 0; -} -// uint8_t MPU6050::dmpGetGyroAndAccelSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGyroSensor(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetControlData(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetTemperature(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetGravity(long *data, const uint8_t* packet); -uint8_t MPU6050::dmpGetGravity(int16_t *data, const uint8_t* packet) { - /* +1g corresponds to +8192, sensitivity is 2g. */ - int16_t qI[4]; - uint8_t status = dmpGetQuaternion(qI, packet); - data[0] = ((int32_t)qI[1] * qI[3] - (int32_t)qI[0] * qI[2]) / 16384; - data[1] = ((int32_t)qI[0] * qI[1] + (int32_t)qI[2] * qI[3]) / 16384; - data[2] = ((int32_t)qI[0] * qI[0] - (int32_t)qI[1] * qI[1] - - (int32_t)qI[2] * qI[2] + (int32_t)qI[3] * qI[3]) / (2 * 16384); - return status; -} - -uint8_t MPU6050::dmpGetGravity(VectorFloat *v, Quaternion *q) { - v -> x = 2 * (q -> x*q -> z - q -> w*q -> y); - v -> y = 2 * (q -> w*q -> x + q -> y*q -> z); - v -> z = q -> w*q -> w - q -> x*q -> x - q -> y*q -> y + q -> z*q -> z; - return 0; -} -// uint8_t MPU6050::dmpGetUnquantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuantizedAccel(long *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetExternalSensorData(long *data, int size, const uint8_t* packet); -// uint8_t MPU6050::dmpGetEIS(long *data, const uint8_t* packet); - -uint8_t MPU6050::dmpGetEuler(float *data, Quaternion *q) { - 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 - data[1] = -asin(2*q -> x*q -> z + 2*q -> w*q -> y); // theta - 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 - return 0; -} - -#ifdef USE_OLD_DMPGETYAWPITCHROLL -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan(gravity -> x / sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan(gravity -> y / sqrt(gravity -> x*gravity -> x + gravity -> z*gravity -> z)); - return 0; -} -#else -uint8_t MPU6050::dmpGetYawPitchRoll(float *data, Quaternion *q, VectorFloat *gravity) { - // yaw: (about Z axis) - data[0] = atan2(2*q -> x*q -> y - 2*q -> w*q -> z, 2*q -> w*q -> w + 2*q -> x*q -> x - 1); - // pitch: (nose up/down, about Y axis) - data[1] = atan2(gravity -> x , sqrt(gravity -> y*gravity -> y + gravity -> z*gravity -> z)); - // roll: (tilt left/right, about X axis) - data[2] = atan2(gravity -> y , gravity -> z); - if(gravity->z<0) { - if(data[1]>0) { - data[1] = PI - data[1]; - } else { - data[1] = -PI - data[1]; - } - } - return 0; -} -#endif - -// uint8_t MPU6050::dmpGetAccelFloat(float *data, const uint8_t* packet); -// uint8_t MPU6050::dmpGetQuaternionFloat(float *data, const uint8_t* packet); - -uint8_t MPU6050::dmpProcessFIFOPacket(const unsigned char *dmpData) { - /*for (uint8_t k = 0; k < dmpPacketSize; k++) { - if (dmpData[k] < 0x10) Serial.print("0"); - Serial.print(dmpData[k], HEX); - Serial.print(" "); - } - Serial.print("\n");*/ - //Serial.println((uint16_t)dmpPacketBuffer); - return 0; -} -uint8_t MPU6050::dmpReadAndProcessFIFOPacket(uint8_t numPackets, uint8_t *processed) { - uint8_t status; - uint8_t buf[dmpPacketSize]; - for (uint8_t i = 0; i < numPackets; i++) { - // read packet from FIFO - getFIFOBytes(buf, dmpPacketSize); - - // process packet - if ((status = dmpProcessFIFOPacket(buf)) > 0) return status; - - // increment external process count variable, if supplied - if (processed != 0) *processed++; - } - return 0; -} - -// uint8_t MPU6050::dmpSetFIFOProcessedCallback(void (*func) (void)); - -// uint8_t MPU6050::dmpInitFIFOParam(); -// uint8_t MPU6050::dmpCloseFIFO(); -// uint8_t MPU6050::dmpSetGyroDataSource(uint_fast8_t source); -// uint8_t MPU6050::dmpDecodeQuantizedAccel(); -// uint32_t MPU6050::dmpGetGyroSumOfSquare(); -// uint32_t MPU6050::dmpGetAccelSumOfSquare(); -// void MPU6050::dmpOverrideQuaternion(long *q); -uint16_t MPU6050::dmpGetFIFOPacketSize() { - return dmpPacketSize; -} +typedef MPU6050_9Axis_MotionApps41 MPU6050; -#endif /* _MPU6050_9AXIS_MOTIONAPPS41_H_ */ +#endif /* _MPU6050_6AXIS_MOTIONAPPS41_H_ */