使いやすいように。
Dependencies: ArduinoSerial I2Cdev
Fork of MPU6050 by
Revision 4:a1c169138c71, committed 2013-11-20
- Comitter:
- syundo0730
- Date:
- Wed Nov 20 17:58:11 2013 +0000
- Parent:
- 3:25e1a5a10e53
- Child:
- 5:7d1bf3ce0053
- Commit message:
- debugging message was modified
Changed in this revision
MPU6050_6Axis_MotionApps20.h | Show annotated file Show diff for this revision Revisions of this file |
MPU6050_6Axis_MotionApps20old.h | Show diff for this revision Revisions of this file |
--- a/MPU6050_6Axis_MotionApps20.h Tue Nov 19 19:05:55 2013 +0000 +++ b/MPU6050_6Axis_MotionApps20.h Wed Nov 20 17:58:11 2013 +0000 @@ -33,6 +33,8 @@ #ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ #define _MPU6050_6AXIS_MOTIONAPPS20_H_ +#include <iostream> + #include "I2Cdev.h" #include "helper_3dmath.h" @@ -100,12 +102,13 @@ // after moving string constants to flash memory storage using the F() // compiler macro (Arduino IDE 1.0+ required). -//#define DEBUG +#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) + #define DEBUG_PRINT(x) std::cout << x //Serial.print(x) + #define DEBUG_PRINTF(x, y) std::cout << x //Serial.print(x, y) + #define DEBUG_PRINTLN(x) std::cout << x << std::endl //Serial.println(x) + #define DEBUG_PRINTLNF(x, y) std::cout << x << std::endl //Serial.println(x, y) + #define F(x) x #else #define DEBUG_PRINT(x) #define DEBUG_PRINTF(x, y) @@ -355,19 +358,19 @@ 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!")); + DEBUG_PRINTLN((otpValid ? F("valid!") : F("invalid!"))); // get X/Y/Z gyro offsets DEBUG_PRINTLN(F("Reading gyro offset TC values...")); //int8_t xgOffsetTC = getXGyroOffsetTC(); //int8_t ygOffsetTC = getYGyroOffsetTC(); //int8_t zgOffsetTC = getZGyroOffsetTC(); - 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); + //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); // setup weird slave stuff (?) DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F..."));
--- a/MPU6050_6Axis_MotionApps20old.h Tue Nov 19 19:05:55 2013 +0000 +++ /dev/null Thu Jan 01 00:00:00 1970 +0000 @@ -1,767 +0,0 @@ -// 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) -// 6/18/2012 by Jeff Rowberg <jeff@rowberg.net> -// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib -// -// Changelog: -// ... - ongoing debug release - -/* ============================================ -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. -=============================================== -*/ - -extern Serial pc; - -#ifndef _MPU6050_6AXIS_MOTIONAPPS20_H_ -#define _MPU6050_6AXIS_MOTIONAPPS20_H_ - -#include "mbed.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 - -#include "MPU6050.h" - -/* 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) printf("%f", x) //Serial.print(x,) - #define DEBUG_PRINTF(x, y) printf("%f, %f", x, y) //Serial.print(x, y) - #define DEBUG_PRINTLN(x) printf("%f\r\n", x) //Serial.println(x) - #define DEBUG_PRINTLNF(x, y) printf("%f, %f\r\n", 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[] - -//To invalid Arduino Memory - -typedef unsigned char prog_uchar; -#define PROGMEM - -/* ================================================================================================ * - | 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) -const prog_uchar 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, 0x00, 0x00, 0x00, - 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - - // bank 3, 256 bytes - 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, 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, 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, 256 bytes - 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, 256 bytes - 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, 256 bytes - 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, 138 bytes (remainder) - 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, 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 -}; - -// thanks to Noah Zerkin for piecing this stuff together! -const prog_uchar dmpConfig[MPU6050_DMP_CONFIG_SIZE] PROGMEM = { -// BANK OFFSET LENGTH [DATA] - 0x03, 0x7B, 0x03, 0x4C, 0xCD, 0x6C, // FCFG_1 inv_set_gyro_calibration - 0x03, 0xAB, 0x03, 0x36, 0x56, 0x76, // FCFG_3 inv_set_gyro_calibration - 0x00, 0x68, 0x04, 0x02, 0xCB, 0x47, 0xA2, // D_0_104 inv_set_gyro_calibration - 0x02, 0x18, 0x04, 0x00, 0x05, 0x8B, 0xC1, // D_0_24 inv_set_gyro_calibration - 0x01, 0x0C, 0x04, 0x00, 0x00, 0x00, 0x00, // D_1_152 inv_set_accel_calibration - 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_accel_calibration - 0x03, 0x89, 0x03, 0x26, 0x46, 0x66, // FCFG_7 inv_set_accel_calibration - 0x00, 0x6C, 0x02, 0x20, 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, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_01 - 0x02, 0x48, 0x04, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_02 - 0x02, 0x4C, 0x04, 0x00, 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, 0x00, 0x00, 0x00, 0x00, // CPASS_MTX_22 - 0x01, 0xEC, 0x04, 0x00, 0x00, 0x40, 0x00, // D_1_236 inv_apply_endian_accel - 0x03, 0x7F, 0x06, 0x0C, 0xC9, 0x2C, 0x97, 0x97, 0x97, // FCFG_2 inv_set_mpu_sensors - 0x04, 0x02, 0x03, 0x0D, 0x35, 0x5D, // CFG_MOTION_BIAS inv_turn_on_bias_from_no_motion - 0x04, 0x09, 0x04, 0x87, 0x2D, 0x35, 0x3D, // FCFG_5 inv_set_bias_update - 0x00, 0xA3, 0x01, 0x00, // D_0_163 inv_set_dead_zone - // SPECIAL 0x01 = enable interrupts - 0x00, 0x00, 0x00, 0x01, // SET INT_ENABLE at i=22, SPECIAL INSTRUCTION - 0x07, 0x86, 0x01, 0xFE, // CFG_6 inv_set_fifo_interupt - 0x07, 0x41, 0x05, 0xF1, 0x20, 0x28, 0x30, 0x38, // CFG_8 inv_send_quaternion - 0x07, 0x7E, 0x01, 0x30, // CFG_16 inv_set_footer - 0x07, 0x46, 0x01, 0x9A, // CFG_GYRO_SOURCE inv_send_gyro - 0x07, 0x47, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_9 inv_send_gyro -> inv_construct3_fifo - 0x07, 0x6C, 0x04, 0xF1, 0x28, 0x30, 0x38, // CFG_12 inv_send_accel -> inv_construct3_fifo - 0x02, 0x16, 0x02, 0x00, 0x01 // D_0_22 inv_set_fifo_rate - - // This very last 0x01 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 prog_uchar dmpUpdates[MPU6050_DMP_UPDATES_SIZE] PROGMEM = { - 0x01, 0xB2, 0x02, 0xFF, 0xFF, - 0x01, 0x90, 0x04, 0x09, 0x23, 0xA1, 0x35, - 0x01, 0x6A, 0x02, 0x06, 0x00, - 0x01, 0x60, 0x08, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x40, 0x00, 0x00, 0x00, - 0x01, 0x62, 0x02, 0x00, 0x00, - 0x00, 0x60, 0x04, 0x00, 0x40, 0x00, 0x00 -}; - -uint8_t MPU6050::dmpInitialize() { - // reset device - DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); - pc.printf("Resetting MPU6050...\r\n"); - reset(); - wait_ms(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 - DEBUG_PRINTLN(F("Disabling sleep mode...")); - pc.printf("Disabling sleep mode...\r\n"); - setSleepEnabled(false); - - // 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!")); - if (otpValid) pc.printf("OTP bank is valid!\r\n"); - else pc.printf("OTP bank is invalid!\r\n"); - - // 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); - - // setup weird slave stuff (?) - DEBUG_PRINTLN(F("Setting slave 0 address to 0x7F...")); - pc.printf("Setting slave 0 address to 0x7F...\r\n"); - setSlaveAddress(0, 0x7F); - DEBUG_PRINTLN(F("Disabling I2C Master mode...")); - pc.printf("Disabling I2C Master mode...\r\n"); - setI2CMasterModeEnabled(false); - DEBUG_PRINTLN(F("Setting slave 0 address to 0x68 (self)...")); - pc.printf("Setting slave 0 address to 0x68 (self)...\r\n"); - setSlaveAddress(0, 0x68); - DEBUG_PRINTLN(F("Resetting I2C Master control...")); - pc.printf("Resetting I2C Master control...\r\n"); - resetI2CMaster(); - wait_ms(20); - - // load DMP code into memory banks - DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); - pc.printf("Writing DMP code to MPU memory banks ("); - DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); - pc.printf("%d", MPU6050_DMP_CODE_SIZE); - DEBUG_PRINTLN(F(" bytes)")); - pc.printf(" bytes)\r\n"); - if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { - DEBUG_PRINTLN(F("Success! DMP code written and verified.")); - pc.printf("Success! DMP code written and verified.\r\n"); - - // 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.")); - pc.printf("Success! DMP configuration written and verified.\r\n"); - - DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); - pc.printf("Setting clock source to Z Gyro...\r\n"); - setClockSource(MPU6050_CLOCK_PLL_ZGYRO); - - DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); - pc.printf("Setting DMP and FIFO_OFLOW interrupts enabled...\r\n"); - setIntEnabled(0x12); - - DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); - pc.printf("Setting sample rate to 200Hz...\r\n"); - setRate(4); // 1khz / (1 + 4) = 200 Hz - - DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); - pc.printf("Setting external frame sync to TEMP_OUT_L[0]...\r\n"); - setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); - - DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); - pc.printf("Setting DLPF bandwidth to 42Hz...\r\n"); - setDLPFMode(MPU6050_DLPF_BW_42); - - DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); - pc.printf("Setting gyro sensitivity to +/- 2000 deg/sec...\r\n"); - setFullScaleGyroRange(MPU6050_GYRO_FS_2000); - - DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); - pc.printf("Setting DMP configuration bytes (function unknown)...\r\n"); - setDMPConfig1(0x03); - setDMPConfig2(0x00); - - DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); - pc.printf("Clearing OTP Bank flag...\r\n"); - setOTPBankValid(false); - - DEBUG_PRINTLN(F("Setting X/Y/Z gyro offsets to previous values...")); - pc.printf("Setting X/Y/Z gyro offsets to previous values...\r\n"); - setXGyroOffset(xgOffset); - setYGyroOffset(ygOffset); - setZGyroOffset(zgOffset); - - DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); - pc.printf("Setting X/Y/Z gyro user offsets to zero...\r\n"); - setXGyroOffsetUser(0); - setYGyroOffsetUser(0); - setZGyroOffsetUser(0); - - DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); - pc.printf("Writing final memory update 1/7 (function unknown)...\r\n"); - uint8_t dmpUpdate[16], j; - uint16_t pos = 0; - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; //pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 2/7 (function unknown)...")); - pc.printf("Writing final memory update 2/7 (function unknown)...\r\n"); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; //pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Resetting FIFO...")); - pc.printf("Resetting FIFO...\r\n"); - resetFIFO(); - - DEBUG_PRINTLN(F("Reading FIFO count...")); - pc.printf("Reading FIFO count...\r\n"); - uint8_t fifoCount = getFIFOCount(); - uint8_t fifoBuffer[128]; - - DEBUG_PRINT(F("Current FIFO count=")); - pc.printf("Current FIFO count="); - DEBUG_PRINTLN(fifoCount); - pc.printf("%d\r\n", fifoCount); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); - pc.printf("Setting motion detection threshold to 2...\r\n"); - setMotionDetectionThreshold(2); - - DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); - pc.printf("Setting zero-motion detection threshold to 156...\r\n"); - setZeroMotionDetectionThreshold(156); - - DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); - pc.printf("Setting motion detection duration to 80...\r\n"); - setMotionDetectionDuration(80); - - DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); - pc.printf("Setting zero-motion detection duration to 0...\r\n"); - setZeroMotionDetectionDuration(0); - - DEBUG_PRINTLN(F("Resetting FIFO...")); - pc.printf("Resetting FIFO...\r\n"); - resetFIFO(); - - DEBUG_PRINTLN(F("Enabling FIFO...")); - pc.printf("Enabling FIFO...\r\n"); - setFIFOEnabled(true); - - DEBUG_PRINTLN(F("Enabling DMP...")); - pc.printf("Enabling DMP...\r\n"); - setDMPEnabled(true); - - DEBUG_PRINTLN(F("Resetting DMP...")); - pc.printf("Resetting DMP...\r\n"); - resetDMP(); - - DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)...")); - pc.printf("Writing final memory update 3/7 (function unknown)...\r\n"); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; //pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 4/7 (function unknown)...")); - pc.printf("Writing final memory update 4/7 (function unknown)...\r\n"); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; //pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Writing final memory update 5/7 (function unknown)...")); - pc.printf("Writing final memory update 5/7 (function unknown)...\r\n"); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; //pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); - pc.printf("Waiting for FIFO count > 2...\r\n"); - while ((fifoCount = getFIFOCount()) < 3); - - DEBUG_PRINT(F("Current FIFO count=")); - pc.printf("Current FIFO count="); - DEBUG_PRINTLN(fifoCount); - pc.printf("%d\r\n", fifoCount); - DEBUG_PRINTLN(F("Reading FIFO data...")); - pc.printf("Reading FIFO data...\r\n"); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Reading interrupt status...")); - pc.printf("Reading interrupt status...\r\n"); - uint8_t mpuIntStatus = getIntStatus(); - - DEBUG_PRINT(F("Current interrupt status=")); - pc.printf("Current interrupt status="); - DEBUG_PRINTLNF(mpuIntStatus, HEX); - pc.printf("%x\r\n", mpuIntStatus); - - DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); - pc.printf("Reading final memory update 6/7 (function unknown)...\r\n"); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; //pgm_read_byte(&dmpUpdates[pos]); - readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); - pc.printf("Waiting for FIFO count > 2...\r\n"); - while ((fifoCount = getFIFOCount()) < 3); - - DEBUG_PRINT(F("Current FIFO count=")); - pc.printf("Current FIFO count="); - DEBUG_PRINTLN(fifoCount); - pc.printf("%d\r\n", fifoCount); - - DEBUG_PRINTLN(F("Reading FIFO data...")); - pc.printf("Reading FIFO data...\r\n"); - getFIFOBytes(fifoBuffer, fifoCount); - - DEBUG_PRINTLN(F("Reading interrupt status...")); - pc.printf("Reading interrupt status...\r\n"); - mpuIntStatus = getIntStatus(); - - DEBUG_PRINT(F("Current interrupt status=")); - pc.printf("Current interrupt status="); - DEBUG_PRINTLNF(mpuIntStatus, HEX); - pc.printf("%x\r\n", mpuIntStatus); - - DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)...")); - pc.printf("Writing final memory update 7/7 (function unknown)...\r\n"); - for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = dmpUpdates[pos]; //pgm_read_byte(&dmpUpdates[pos]); - writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); - - DEBUG_PRINTLN(F("DMP is good to go! Finally.")); - pc.printf("DMP is good to go! Finally.\r\n"); - - DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)...")); - pc.printf("DDisabling DMP (you turn it on later)...\r\n"); - setDMPEnabled(false); - - DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); - pc.printf("Setting up internal 42-byte (default) DMP packet buffer...\r\n"); - dmpPacketSize = 42; - /*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...")); - pc.printf("Resetting FIFO and clearing INT status one last time...\r\n"); - 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] = ((packet[28] << 24) + (packet[29] << 16) + (packet[30] << 8) + packet[31]); - data[1] = ((packet[32] << 24) + (packet[33] << 16) + (packet[34] << 8) + packet[35]); - data[2] = ((packet[36] << 24) + (packet[37] << 16) + (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] = ((packet[0] << 24) + (packet[1] << 16) + (packet[2] << 8) + packet[3]); - data[1] = ((packet[4] << 24) + (packet[5] << 16) + (packet[6] << 8) + packet[7]); - data[2] = ((packet[8] << 24) + (packet[9] << 16) + (packet[10] << 8) + packet[11]); - data[3] = ((packet[12] << 24) + (packet[13] << 16) + (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] = ((packet[16] << 24) + (packet[17] << 16) + (packet[18] << 8) + packet[19]); - data[1] = ((packet[20] << 24) + (packet[21] << 16) + (packet[22] << 8) + packet[23]); - data[2] = ((packet[24] << 24) + (packet[25] << 16) + (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::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(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; -} -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; -} - -// 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; -} - -#endif /* _MPU6050_6AXIS_MOTIONAPPS20_H_ */ \ No newline at end of file