Rolf Ziegler
Complete MPU6050dmp example working with processing 2.1 teapot code v1.0 Jan. 2014
Fork of
MPU6050
by 
Shundo Kishi
Revision 6:029b4b8111ef, committed 2014-01-10
- Comitter:
- rolfz
- Date:
- Fri Jan 10 17:01:28 2014 +0000
- Parent:
- 5:7d1bf3ce0053
- Commit message:
- Complete MBED example with main code, working with Processing v2.1 Teapot example!
Changed in this revision
diff -r 7d1bf3ce0053 -r 029b4b8111ef I2Cdev.h
--- a/I2Cdev.h Sat Nov 23 16:47:00 2013 +0000
+++ b/I2Cdev.h Fri Jan 10 17:01:28 2014 +0000
@@ -9,8 +9,8 @@
#include "mbed.h"
-#define I2C_SDA p28
-#define I2C_SCL p27
+#define I2C_SDA p9
+#define I2C_SCL p10
class I2Cdev {
private:
diff -r 7d1bf3ce0053 -r 029b4b8111ef MPU6050.h --- a/MPU6050.h Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050.h Fri Jan 10 17:01:28 2014 +0000 @@ -48,7 +48,7 @@ #define MPU6050_ADDRESS_AD0_LOW 0x68 // address pin low (GND), default for InvenSense evaluation board #define MPU6050_ADDRESS_AD0_HIGH 0x69 // address pin high (VCC) -#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_LOW +#define MPU6050_DEFAULT_ADDRESS MPU6050_ADDRESS_AD0_HIGH #define MPU6050_RA_XG_OFFS_TC 0x00 //[7] PWR_MODE, [6:1] XG_OFFS_TC, [0] OTP_BNK_VLD #define MPU6050_RA_YG_OFFS_TC 0x01 //[7] PWR_MODE, [6:1] YG_OFFS_TC, [0] OTP_BNK_VLD
diff -r 7d1bf3ce0053 -r 029b4b8111ef MPU6050_6Axis_MotionApps20.h
--- a/MPU6050_6Axis_MotionApps20.h Sat Nov 23 16:47:00 2013 +0000
+++ b/MPU6050_6Axis_MotionApps20.h Fri Jan 10 17:01:28 2014 +0000
@@ -104,16 +104,14 @@
//#define DEBUG
#ifdef DEBUG
- #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 DEBUG_PRINT(x) pc.printf(x) //Serial.print(x)
+ #define DEBUG_PRINTF(x, y) pc.printf(x,y) //Serial.print(x, y)
+
#define F(x) x
#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[]
@@ -327,9 +325,15 @@
uint8_t MPU6050::dmpInitialize() {
// reset device
- DEBUG_PRINTLN(F("\n\nResetting MPU6050..."));
+ Serial pc(USBTX, USBRX); // tx, rx
+ pc.baud(115200);
+ wait_ms(50);
+
+ pc.printf("\n\nSet USB Baudrate to 115200...\n");
+
+ pc.printf("\n\nResetting MPU6050...\n");
reset();
- //delay(30); // wait after reset
+
wait_ms(30);
// enable sleep mode and wake cycle
@@ -339,91 +343,92 @@
setWakeCycleEnabled(true);*/
// disable sleep mode
- DEBUG_PRINTLN(F("Disabling sleep mode..."));
+ DEBUG_PRINT("Disabling sleep mode...\n");
setSleepEnabled(false);
// get MPU hardware revision
- DEBUG_PRINTLN(F("Selecting user bank 16..."));
+ DEBUG_PRINT("Selecting user bank 16...\n");
setMemoryBank(0x10, true, true);
- DEBUG_PRINTLN(F("Selecting memory byte 6..."));
+ DEBUG_PRINT("Selecting memory byte 6...\n");
setMemoryStartAddress(0x06);
- DEBUG_PRINTLN(F("Checking hardware revision..."));
+ DEBUG_PRINT("Checking hardware revision...\n");
uint8_t hwRevision = readMemoryByte();
- DEBUG_PRINT(F("Revision @ user[16][6] = "));
- DEBUG_PRINTLNF(hwRevision, HEX);
- DEBUG_PRINTLN(F("Resetting memory bank selection to 0..."));
+ DEBUG_PRINT("Revision @ user[16][6] = ");
+ DEBUG_PRINTF("%x\n",hwRevision);
+ DEBUG_PRINT("Resetting memory bank selection to 0...\n");
setMemoryBank(0, false, false);
// check OTP bank valid
- DEBUG_PRINTLN(F("Reading OTP bank valid flag..."));
+ DEBUG_PRINT("Reading OTP bank valid flag...\n");
uint8_t otpValid = getOTPBankValid();
- DEBUG_PRINT(F("OTP bank is "));
- DEBUG_PRINTLN((otpValid ? F("valid!") : F("invalid!")));
+
+ DEBUG_PRINT("OTP bank is ");
+ if(otpValid) DEBUG_PRINT("valid!\n");
+ else DEBUG_PRINT("invalid!\n");
// 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("\nReading gyro offset TC values...\n");
+ int8_t xgOffsetTC = mpu.getXGyroOffsetTC();
+ int8_t ygOffsetTC = getYGyroOffsetTC();
+ int8_t zgOffsetTC = getZGyroOffsetTC();
+ DEBUG_PRINTF("X gyro offset = %u\n",xgOffset);
+ DEBUG_PRINTF("Y gyro offset = %u\n",ygOffset);
+ DEBUG_PRINTF("Z gyro offset = %u\n",zgOffset);
+ */
+ // setup weird slave stuff (?)
+ DEBUG_PRINT("Setting slave 0 address to 0x7F...\n");
+ setSlaveAddress(0, 0x7F);
+
+ DEBUG_PRINT("Disabling I2C Master mode...");
+ setI2CMasterModeEnabled(false);
+ DEBUG_PRINT("Setting slave 0 address to 0x68 (self)...");
+ setSlaveAddress(0, 0x68);
+ DEBUG_PRINT("Resetting I2C Master control...\n");
+ resetI2CMaster();
- // 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);
wait_ms(20);
// 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)"));
+ DEBUG_PRINT("Writing DMP code to MPU memory banks (");
+ DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE);
+ DEBUG_PRINT(" bytes)\n");
if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
- DEBUG_PRINTLN(F("Success! DMP code written and verified."));
+ DEBUG_PRINT("Success! DMP code written and verified.\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)"));
+ DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
+ DEBUG_PRINTF("%u",MPU6050_DMP_CONFIG_SIZE);
+ DEBUG_PRINT(" bytes in config def)\n");
if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
- DEBUG_PRINTLN(F("Success! DMP configuration written and verified."));
+ DEBUG_PRINT("Success! DMP configuration written and verified.\n");
- DEBUG_PRINTLN(F("Setting clock source to Z Gyro..."));
+ DEBUG_PRINT("Setting clock source to Z Gyro...\n");
setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
- DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled..."));
+ DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
setIntEnabled(0x12);
- DEBUG_PRINTLN(F("Setting sample rate to 200Hz..."));
+ DEBUG_PRINT("Setting sample rate to 200Hz...");
setRate(4); // 1khz / (1 + 4) = 200 Hz
- DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]..."));
+ DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n");
setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
- DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz..."));
+ DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n");
setDLPFMode(MPU6050_DLPF_BW_42);
- DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec..."));
+ DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
- DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)..."));
+ DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n");
setDMPConfig1(0x03);
setDMPConfig2(0x00);
- DEBUG_PRINTLN(F("Clearing OTP Bank flag..."));
+ DEBUG_PRINT("Clearing OTP Bank flag...");
setOTPBankValid(false);
- DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values..."));
+ DEBUG_PRINT("Setting X/Y/Z gyro offset TCs to previous values...\n");
//setXGyroOffsetTC(xgOffsetTC);
//setYGyroOffsetTC(ygOffsetTC);
//setZGyroOffsetTC(zgOffsetTC);
@@ -433,120 +438,120 @@
//setYGyroOffset(0);
//setZGyroOffset(0);
- DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)..."));
+ DEBUG_PRINT("Writing final memory update 1/7 (function unknown)...\n");
uint8_t dmpUpdate[16], j;
uint16_t pos = 0;
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 2/7 (function unknown)..."));
+ DEBUG_PRINT("Writing final memory update 2/7 (function unknown)...\n");
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("Resetting FIFO..."));
+ DEBUG_PRINT("Resetting FIFO...\n");
resetFIFO();
- DEBUG_PRINTLN(F("Reading FIFO count..."));
+ DEBUG_PRINT("Reading FIFO count...\n");
uint16_t fifoCount = getFIFOCount();
uint8_t fifoBuffer[128];
- DEBUG_PRINT(F("Current FIFO count="));
- DEBUG_PRINTLN(fifoCount);
+ DEBUG_PRINT("Current FIFO count=");
+ DEBUG_PRINTF("%u\n",fifoCount);
getFIFOBytes(fifoBuffer, fifoCount);
- DEBUG_PRINTLN(F("Setting motion detection threshold to 2..."));
+ DEBUG_PRINT("Setting motion detection threshold to 2...\n");
setMotionDetectionThreshold(2);
- DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156..."));
+ DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n");
setZeroMotionDetectionThreshold(156);
- DEBUG_PRINTLN(F("Setting motion detection duration to 80..."));
+ DEBUG_PRINT("Setting motion detection duration to 80...");
setMotionDetectionDuration(80);
- DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0..."));
+ DEBUG_PRINT("Setting zero-motion detection duration to 0...");
setZeroMotionDetectionDuration(0);
- DEBUG_PRINTLN(F("Resetting FIFO..."));
+ DEBUG_PRINT("Resetting FIFO...\n");
resetFIFO();
- DEBUG_PRINTLN(F("Enabling FIFO..."));
+ DEBUG_PRINT("Enabling FIFO...\n");
setFIFOEnabled(true);
- DEBUG_PRINTLN(F("Enabling DMP..."));
+ DEBUG_PRINT("Enabling DMP...\n");
setDMPEnabled(true);
- DEBUG_PRINTLN(F("Resetting DMP..."));
+ DEBUG_PRINT("Resetting DMP...\n");
resetDMP();
- DEBUG_PRINTLN(F("Writing final memory update 3/7 (function unknown)..."));
+ DEBUG_PRINT("Writing final memory update 3/7 (function unknown)...\n");
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 4/7 (function unknown)..."));
+ DEBUG_PRINT("Writing final memory update 4/7 (function unknown)...\n");
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 5/7 (function unknown)..."));
+ DEBUG_PRINT("Writing final memory update 5/7 (function unknown)...\n");
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 FIFO count > 2..."));
+ DEBUG_PRINT("Waiting for FIFO count > 2...\n");
while ((fifoCount = getFIFOCount()) < 3);
- DEBUG_PRINT(F("Current FIFO count="));
- DEBUG_PRINTLN(fifoCount);
- DEBUG_PRINTLN(F("Reading FIFO data..."));
+ DEBUG_PRINT("Current FIFO count=");
+ DEBUG_PRINTF("%u\n",fifoCount);
+ DEBUG_PRINT("Reading FIFO data...\n");
getFIFOBytes(fifoBuffer, fifoCount);
- DEBUG_PRINTLN(F("Reading interrupt status..."));
+ DEBUG_PRINT("Reading interrupt status...\n");
uint8_t mpuIntStatus = getIntStatus();
- DEBUG_PRINT(F("Current interrupt status="));
- DEBUG_PRINTLNF(mpuIntStatus, HEX);
+ DEBUG_PRINT("Current interrupt status=");
+ DEBUG_PRINTF("%x\n",mpuIntStatus);
- DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)..."));
+ DEBUG_PRINT("Reading final memory update 6/7 (function unknown)...\n");
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]);
- DEBUG_PRINTLN(F("Waiting for FIFO count > 2..."));
+ DEBUG_PRINT("Waiting for FIFO count > 2...\n");
while ((fifoCount = getFIFOCount()) < 3);
- DEBUG_PRINT(F("Current FIFO count="));
- DEBUG_PRINTLN(fifoCount);
+ DEBUG_PRINT("Current FIFO count=");
+ DEBUG_PRINTF("%u\n",fifoCount);
- DEBUG_PRINTLN(F("Reading FIFO data..."));
+ DEBUG_PRINT("Reading FIFO data...\n");
getFIFOBytes(fifoBuffer, fifoCount);
- DEBUG_PRINTLN(F("Reading interrupt status..."));
+ DEBUG_PRINT("Reading interrupt status...\n");
mpuIntStatus = getIntStatus();
- DEBUG_PRINT(F("Current interrupt status="));
- DEBUG_PRINTLNF(mpuIntStatus, HEX);
+ DEBUG_PRINT("Current interrupt status=");
+ DEBUG_PRINTF("%x\n",mpuIntStatus);
- DEBUG_PRINTLN(F("Writing final memory update 7/7 (function unknown)..."));
+ DEBUG_PRINT("Writing final memory update 7/7 (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("DMP is good to go! Finally."));
+ DEBUG_PRINT("DMP is good to go! Finally.\n");
- DEBUG_PRINTLN(F("Disabling DMP (you turn it on later)..."));
+ DEBUG_PRINT("Disabling DMP (you turn it on later)...\n");
setDMPEnabled(false);
- DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer..."));
+ DEBUG_PRINT("Setting up internal 42-byte (default) DMP packet buffer...\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..."));
+ DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n");
resetFIFO();
getIntStatus();
} else {
- DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed."));
+ DEBUG_PRINT("ERROR! DMP configuration verification failed.\n");
return 2; // configuration block loading failed
}
} else {
- DEBUG_PRINTLN(F("ERROR! DMP code verification failed."));
+ DEBUG_PRINT("ERROR! DMP code verification failed.");
return 1; // main binary block loading failed
}
return 0; // success
@@ -691,6 +696,7 @@
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);
diff -r 7d1bf3ce0053 -r 029b4b8111ef MPU6050_9Axis_MotionApps41.h
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/MPU6050_9Axis_MotionApps41.h Fri Jan 10 17:01:28 2014 +0000
@@ -0,0 +1,865 @@
+// 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 <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.
+===============================================
+*/
+
+#ifndef _MPU6050_9AXIS_MOTIONAPPS41_H_
+#define _MPU6050_9AXIS_MOTIONAPPS41_H_
+
+#include <iostream>
+
+#include "I2Cdev.h"
+#include "helper_3dmath.h"
+
+// MotionApps 4.1 DMP implementation, built using the MPU-9150 "MotionFit" board
+#define MPU6050_INCLUDE_DMP_MOTIONAPPS41
+
+#include "MPU6050.h"
+
+// Tom Carpenter's conditional PROGMEM code
+// http://forum.arduino.cc/index.php?topic=129407.0
+#ifndef __arm__
+#include <avr/pgmspace.h>
+#else
+ // Teensy 3.0 library conditional PROGMEM code from Paul Stoffregen
+#ifndef __PGMSPACE_H_
+#define __PGMSPACE_H_ 1
+#include <inttypes.h>
+
+#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
+
+#define min(a,b) (((a)<(b))?(a):(b))
+
+// 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) pc.printf(x) //Serial.print(x)
+ #define DEBUG_PRINTF(x, y) pc.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 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, 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
+};
+
+const prog_uchar 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, 0x03 // 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, 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() {
+ Serial pc(USBTX, USBRX); // tx, rx
+ pc.baud(115200);
+ wait_ms(50);
+
+ pc.printf("\n\nSet USB Baudrate to 115200...\n");
+
+ // reset device
+ DEBUG_PRINT("\n\nResetting MPU6050...");
+ reset();
+ wait_ms(30);
+
+ // disable sleep mode
+ DEBUG_PRINT("Disabling sleep mode...");
+ setSleepEnabled(false);
+
+ // get MPU product ID
+// DEBUG_PRINT("Getting product ID...");
+// uint8_t productID = 0;
+// getProductID();
+// DEBUG_PRINT("Product ID = ");
+// DEBUG_PRINTF("%u", productID);
+
+ // get MPU hardware revision
+ DEBUG_PRINT("Selecting user bank 16...\n");
+ setMemoryBank(0x10, true, true);
+ DEBUG_PRINT("Selecting memory byte 6...\n");
+ setMemoryStartAddress(0x06);
+ DEBUG_PRINT("Checking hardware revision...\n");
+ uint8_t hwRevision = readMemoryByte();
+ DEBUG_PRINT("Revision @ user[16][6] = ");
+ DEBUG_PRINTF("%x\n",hwRevision);
+ DEBUG_PRINT("Resetting memory bank selection to 0...");
+ setMemoryBank(0, false, false);
+
+ // check OTP bank valid
+ DEBUG_PRINT("Reading OTP bank valid flag...\n");
+ uint8_t otpValid = getOTPBankValid();
+ DEBUG_PRINT("OTP bank is ");
+
+ if(otpValid)DEBUG_PRINT("valid!\n");
+ else DEBUG_PRINT("invalid!\n");
+
+ // get X/Y/Z gyro offsets
+ DEBUG_PRINT("Reading gyro offset values...\n");
+ int8_t xgOffset = getXGyroOffset();
+ int8_t ygOffset = getYGyroOffset();
+ int8_t zgOffset = getZGyroOffset();
+ DEBUG_PRINT("X gyro offset = ");
+ DEBUG_PRINTF("%u\n",xgOffset);
+ DEBUG_PRINT("Y gyro offset = ");
+ DEBUG_PRINTF("%u\n",ygOffset);
+ DEBUG_PRINT("Z gyro offset = ");
+ DEBUG_PRINTF("%u\n",zgOffset);
+
+ i2Cdev.readByte(devAddr, MPU6050_RA_USER_CTRL, buffer); // ?
+
+ DEBUG_PRINT("Enabling interrupt latch, clear on any read, AUX bypass enabled\n");
+ i2Cdev.writeByte(devAddr, MPU6050_RA_INT_PIN_CFG, 0x32);
+
+ // enable MPU AUX I2C bypass mode
+ //DEBUG_PRINT("Enabling AUX I2C bypass mode..."));
+ //setI2CBypassEnabled(true);
+
+ DEBUG_PRINT("Setting magnetometer mode to power-down...\n");
+ //mag -> setMode(0);
+ i2Cdev.writeByte(0x0E, 0x0A, 0x00);
+
+ DEBUG_PRINT("Setting magnetometer mode to fuse access...\n");
+ //mag -> setMode(0x0F);
+ i2Cdev.writeByte(0x0E, 0x0A, 0x0F);
+
+ DEBUG_PRINT("Reading mag magnetometer factory calibration...\n");
+
+ 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("Adjustment X/Y/Z = ");
+ DEBUG_PRINTF("%u",asax);
+ DEBUG_PRINT(" / ");
+ DEBUG_PRINTF("%u",asay);
+ DEBUG_PRINT(" / ");
+ DEBUG_PRINTF("%u\n",asaz);
+
+ DEBUG_PRINT("Setting magnetometer mode to power-down...\n");
+ //mag -> setMode(0);
+ i2Cdev.writeByte(0x0E, 0x0A, 0x00);
+
+ // load DMP code into memory banks
+ DEBUG_PRINT("Writing DMP code to MPU memory banks (");
+ DEBUG_PRINTF("%u",MPU6050_DMP_CODE_SIZE);
+ DEBUG_PRINT(" bytes)\n");
+ if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) {
+ DEBUG_PRINT("Success! DMP code written and verified.\n");
+
+ DEBUG_PRINT("Configuring DMP and related settings...\n");
+
+ // write DMP configuration
+ DEBUG_PRINT("Writing DMP configuration to MPU memory banks (");
+ DEBUG_PRINTF("%u ",MPU6050_DMP_CONFIG_SIZE);
+ DEBUG_PRINT(" bytes in config def)\n");
+ if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) {
+ DEBUG_PRINT("Success! DMP configuration written and verified.\n");
+
+ DEBUG_PRINT("Setting DMP and FIFO_OFLOW interrupts enabled...\n");
+ setIntEnabled(0x12);
+
+ DEBUG_PRINT("Setting sample rate to 200Hz...\n");
+ setRate(4); // 1khz / (1 + 4) = 200 Hz
+
+ DEBUG_PRINT("Setting clock source to Z Gyro...\n");
+ setClockSource(MPU6050_CLOCK_PLL_ZGYRO);
+
+ DEBUG_PRINT("Setting DLPF bandwidth to 42Hz...\n");
+ setDLPFMode(MPU6050_DLPF_BW_42);
+
+ DEBUG_PRINT("Setting external frame sync to TEMP_OUT_L[0]...\n");
+ setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L);
+
+ DEBUG_PRINT("Setting gyro sensitivity to +/- 2000 deg/sec...\n");
+ setFullScaleGyroRange(MPU6050_GYRO_FS_2000);
+
+ DEBUG_PRINT("Setting DMP configuration bytes (function unknown)...\n");
+ setDMPConfig1(0x03);
+ setDMPConfig2(0x00);
+
+ DEBUG_PRINT("Clearing OTP Bank flag...\n");
+ setOTPBankValid(false);
+
+ DEBUG_PRINT("Setting X/Y/Z gyro offsets to previous values...\n");
+ setXGyroOffset(xgOffset);
+ setYGyroOffset(ygOffset);
+ setZGyroOffset(zgOffset);
+
+ DEBUG_PRINT("Setting X/Y/Z gyro user offsets to zero...\n");
+ setXGyroOffsetUser(0);
+ setYGyroOffsetUser(0);
+ setZGyroOffsetUser(0);
+
+ DEBUG_PRINT("Writing final memory update 1/19 (function unknown)...\n");
+ uint8_t dmpUpdate[16],j;
+ uint16_t pos = 0;
+ 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_PRINT("Writing final memory update 2/19 (function unknown)...\n");
+ 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_PRINT("Resetting FIFO...\n");
+ resetFIFO();
+
+ DEBUG_PRINT("Reading FIFO count...\n");
+ uint8_t fifoCount = getFIFOCount();
+
+ DEBUG_PRINT("Current FIFO count=");
+ DEBUG_PRINTF("%u\n",fifoCount);
+ uint8_t fifoBuffer[128];
+ getFIFOBytes(fifoBuffer, fifoCount);
+
+ DEBUG_PRINT("Writing final memory update 3/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 4/19 (function unknown)...\n");
+ 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_PRINT("Disabling all standby flags...\n");
+ i2Cdev.writeByte(0x68, MPU6050_RA_PWR_MGMT_2, 0x00);
+
+ DEBUG_PRINT("Setting accelerometer sensitivity to +/- 2g...\n");
+ i2Cdev.writeByte(0x68, MPU6050_RA_ACCEL_CONFIG, 0x00);
+
+ DEBUG_PRINT("Setting motion detection threshold to 2...\n");
+ setMotionDetectionThreshold(2);
+
+ DEBUG_PRINT("Setting zero-motion detection threshold to 156...\n");
+ setZeroMotionDetectionThreshold(156);
+
+ DEBUG_PRINT("Setting motion detection duration to 80...\n");
+ setMotionDetectionDuration(80);
+
+ DEBUG_PRINT("Setting zero-motion detection duration to 0...\n");
+ setZeroMotionDetectionDuration(0);
+
+ DEBUG_PRINT("Setting AK8975 to single measurement mode...\n");
+ //mag -> setMode(1);
+ i2Cdev.writeByte(0x0E, 0x0A, 0x01);
+
+ // setup AK8975 (0x0E) as Slave 0 in read mode
+ DEBUG_PRINT("Setting up AK8975 read slave 0...\n");
+ 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_PRINT("Setting up AK8975 write slave 2...\n");
+ 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_PRINT("Setting up slave access delay...\n");
+ i2Cdev.writeByte(0x68, MPU6050_RA_I2C_SLV4_CTRL, 0x18);
+ i2Cdev.writeByte(0x68, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05);
+
+ // enable interrupts
+ DEBUG_PRINT("Enabling default interrupt behavior/no bypass...\n");
+ i2Cdev.writeByte(0x68, MPU6050_RA_INT_PIN_CFG, 0x00);
+
+ // enable I2C master mode and reset DMP/FIFO
+ DEBUG_PRINT("Enabling I2C master mode...\n");
+ i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
+ DEBUG_PRINT("Resetting FIFO...");
+ i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0x24);
+ DEBUG_PRINT("Rewriting I2C master mode enabled because...I don't know\n");
+ i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0x20);
+ DEBUG_PRINT("Enabling and resetting DMP/FIFO...\n");
+ i2Cdev.writeByte(0x68, MPU6050_RA_USER_CTRL, 0xE8);
+
+ DEBUG_PRINT("Writing final memory update 5/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 6/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 7/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 8/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 9/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 10/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 11/19 (function unknown)...\n");
+ 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_PRINT("Reading final memory update 12/19 (function unknown)...\n");
+ 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("Read bytes: ");
+ for (j = 0; j < 4; j++) {
+ DEBUG_PRINTF("%u",dmpUpdate[3 + j]);
+ DEBUG_PRINT("\n");
+ }
+ DEBUG_PRINT("\n");
+#endif
+
+ DEBUG_PRINT("Writing final memory update 13/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 14/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 15/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 16/19 (function unknown)...\n");
+ 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_PRINT("Writing final memory update 17/19 (function unknown)...\n");
+ 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_PRINT("Waiting for FIFO count >= 46...\n");
+ while ((fifoCount = getFIFOCount()) < 46);
+ DEBUG_PRINT("Reading FIFO...\n");
+ getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
+ DEBUG_PRINT("Reading interrupt status...\n");
+ getIntStatus();
+
+ DEBUG_PRINT("Writing final memory update 18/19 (function unknown)...\n");
+ 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_PRINT("Waiting for FIRO count >= 48...\n");
+ while ((fifoCount = getFIFOCount()) < 48);
+ DEBUG_PRINT("Reading FIFO...");
+ getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
+ DEBUG_PRINT("Reading interrupt status...\n");
+ getIntStatus();
+ DEBUG_PRINT("Waiting for FIRO count >= 48...\n");
+ while ((fifoCount = getFIFOCount()) < 48);
+ DEBUG_PRINT("Reading FIFO...\n");
+ getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes
+ DEBUG_PRINT("Reading interrupt status...\n");
+ getIntStatus();
+
+ DEBUG_PRINT("Writing final memory update 19/19 (function unknown)...\n");
+ 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_PRINT("Disabling DMP (you turn it on later)...\n");
+ setDMPEnabled(false);
+
+ DEBUG_PRINT("Setting up internal 48-byte (default) DMP packet buffer...\n");
+ dmpPacketSize = 48;
+ /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) {
+return 3; // TODO: proper error code for no memory
+}*/
+
+ DEBUG_PRINT("Resetting FIFO and clearing INT status one last time...\n");
+ resetFIFO();
+ getIntStatus();
+ } else {
+ DEBUG_PRINT("ERROR! DMP configuration verification failed.\n");
+ return 2; // configuration block loading failed
+ }
+ } else {
+ DEBUG_PRINT("ERROR! DMP code verification failed.\n");
+ 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[34] << 24) + (packet[35] << 16) + (packet[36] << 8) + packet[37]);
+ data[1] = ((packet[38] << 24) + (packet[39] << 16) + (packet[40] << 8) + packet[41]);
+ data[2] = ((packet[42] << 24) + (packet[43] << 16) + (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] = ((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::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(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_9AXIS_MOTIONAPPS41_H_ */
diff -r 7d1bf3ce0053 -r 029b4b8111ef main.cpp
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp Fri Jan 10 17:01:28 2014 +0000
@@ -0,0 +1,391 @@
+// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class using DMP (MotionApps v2.0)
+// 6/21/2012 by Jeff Rowberg <jeff@rowberg.net>
+// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
+//
+// Changelog:
+// 2013-05-08 - added seamless Fastwire support
+// - added note about gyro calibration
+// 2012-06-21 - added note about Arduino 1.0.1 + Leonardo compatibility error
+// 2012-06-20 - improved FIFO overflow handling and simplified read process
+// 2012-06-19 - completely rearranged DMP initialization code and simplification
+// 2012-06-13 - pull gyro and accel data from FIFO packet instead of reading directly
+// 2012-06-09 - fix broken FIFO read sequence and change interrupt detection to RISING
+// 2012-06-05 - add gravity-compensated initial reference frame acceleration output
+// - add 3D math helper file to DMP6 example sketch
+// - add Euler output and Yaw/Pitch/Roll output formats
+// 2012-06-04 - remove accel offset clearing for better results (thanks Sungon Lee)
+// 2012-06-01 - fixed gyro sensitivity to be 2000 deg/sec instead of 250
+// 2012-05-30 - basic DMP initialization working
+
+/* ============================================
+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.
+===============================================
+*/
+
+// I2Cdev and MPU6050 must be installed as libraries, or else the .cpp/.h files
+// for both classes must be in the include path of your project
+#include "I2Cdev.h"
+#include "mbed.h"
+#include <math.h>
+DigitalOut leds[] = {(LED1), (LED2),(LED3),(LED4)};
+
+
+#include "MPU6050_6Axis_MotionApps20.h" // works
+//#include "MPU6050_9Axis_MotionApps41.h"
+
+//#include "MPU6050.h" // not necessary if using MotionApps include file
+
+
+// class default I2C address is 0x68
+// specific I2C addresses may be passed as a parameter here
+// AD0 low = 0x68 (default for SparkFun breakout and InvenSense evaluation board)
+// AD0 high = 0x69
+MPU6050 mpu;
+//MPU6050 mpu(0x69); // <-- use for AD0 high
+
+/* =========================================================================
+NOTE: In addition to connection 3.3v, GND, SDA, and SCL, this sketch
+depends on the MPU-6050's INT pin being connected to the Arduino's
+external interrupt #0 pin. On the Arduino Uno and Mega 2560, this is
+digital I/O pin 2.
+* ========================================================================= */
+
+/* =========================================================================
+NOTE: Arduino v1.0.1 with the Leonardo board generates a compile error
+when using Serial.write(buf, len). The Teapot output uses this method.
+The solution requires a modification to the Arduino USBAPI.h file, which
+is fortunately simple, but annoying. This will be fixed in the next IDE
+release. For more info, see these links:
+
+http://arduino.cc/forum/index.php/topic,109987.0.html
+http://code.google.com/p/arduino/issues/detail?id=958
+* ========================================================================= */
+
+
+#ifndef M_PI
+#define M_PI 3.1415
+#endif
+
+// uncomment "OUTPUT_READABLE_QUATERNION" if you want to see the actual
+// quaternion components in a [w, x, y, z] format (not best for parsing
+// on a remote host such as Processing or something though)
+//#define OUTPUT_READABLE_QUATERNION
+
+// uncomment "OUTPUT_READABLE_EULER" if you want to see Euler angles
+// (in degrees) calculated from the quaternions coming from the FIFO.
+// Note that Euler angles suffer from gimbal lock (for more info, see
+// http://en.wikipedia.org/wiki/Gimbal_lock)
+//#define OUTPUT_READABLE_EULER
+
+// uncomment "OUTPUT_READABLE_YAWPITCHROLL" if you want to see the yaw/
+// pitch/roll angles (in degrees) calculated from the quaternions coming
+// from the FIFO. Note this also requires gravity vector calculations.
+// Also note that yaw/pitch/roll angles suffer from gimbal lock (for
+// more info, see: http://en.wikipedia.org/wiki/Gimbal_lock)
+//#define OUTPUT_READABLE_YAWPITCHROLL
+
+// uncomment "OUTPUT_READABLE_REALACCEL" if you want to see acceleration
+// components with gravity removed. This acceleration reference frame is
+// not compensated for orientation, so +X is always +X according to the
+// sensor, just without the effects of gravity. If you want acceleration
+// compensated for orientation, us OUTPUT_READABLE_WORLDACCEL instead.
+//#define OUTPUT_READABLE_REALACCEL
+
+// uncomment "OUTPUT_READABLE_WORLDACCEL" if you want to see acceleration
+// components with gravity removed and adjusted for the world frame of
+// reference (yaw is relative to initial orientation, since no magnetometer
+// is present in this case). Could be quite handy in some cases.
+//#define OUTPUT_READABLE_WORLDACCEL
+
+// uncomment "OUTPUT_TEAPOT" if you want output that matches the
+// format used for the InvenSense teapot demo
+#define OUTPUT_TEAPOT
+
+
+
+bool blinkState = false;
+
+// MPU control/status vars
+bool dmpReady = false; // set true if DMP init was successful
+uint8_t mpuIntStatus; // holds actual interrupt status byte from MPU
+uint8_t devStatus; // return status after each device operation (0 = success, !0 = error)
+uint16_t packetSize; // expected DMP packet size (default is 42 bytes)
+uint16_t fifoCount; // count of all bytes currently in FIFO
+uint8_t fifoBuffer[64]; // FIFO storage buffer
+
+// orientation/motion vars
+Quaternion q; // [w, x, y, z] quaternion container
+VectorInt16 aa; // [x, y, z] accel sensor measurements
+VectorInt16 aaReal; // [x, y, z] gravity-free accel sensor measurements
+VectorInt16 aaWorld; // [x, y, z] world-frame accel sensor measurements
+VectorFloat gravity; // [x, y, z] gravity vector
+float euler[3]; // [psi, theta, phi] Euler angle container
+float ypr[3]; // [yaw, pitch, roll] yaw/pitch/roll container and gravity vector
+
+// packet structure for InvenSense teapot demo
+uint8_t teapotPacket[15] = { '$', 0x02, 0,0, 0,0, 0,0, 0,0, 0x00, 0x00, '\r', '\n',0 };
+
+// ================================================================
+// === INTERRUPT DETECTION ROUTINE ===
+// ================================================================
+
+volatile bool mpuInterrupt = false; // indicates whether MPU interrupt pin has gone high
+void dmpDataReady() {
+ mpuInterrupt = true;
+}
+
+// ================================================================
+// === INITIAL SETUP ===
+// ================================================================
+
+int main()
+{
+
+ //Pin Defines for I2C Bus
+#define D_SDA p9
+#define D_SCL p10
+//#define D_SDA p28
+//#define D_SCL p27
+I2C i2c(D_SDA, D_SCL);
+
+
+// mbed Interface Hardware definitions
+ DigitalOut myled1(LED1);
+ DigitalOut myled2(LED2);
+ DigitalOut myled3(LED3);
+ DigitalOut heartbeatLED(LED4);
+
+// initialize serial communication
+// (115200 chosen because it is required for Teapot Demo output, but it's
+// really up to you depending on your project)
+//Host PC Baudrate (Virtual Com Port on USB)
+ #define D_BAUDRATE 115200
+
+// Host PC Communication channels
+ Serial pc(USBTX, USBRX); // tx, rx
+
+ pc.baud(D_BAUDRATE);
+ // initialize device
+ pc.printf("Initializing I2C devices...\n");
+ mpu.initialize();
+
+ // verify connection
+ pc.printf("Testing device connections...\n");
+
+ bool mpu6050TestResult = mpu.testConnection();
+ if(mpu6050TestResult){
+ pc.printf("MPU6050 test passed \n");
+ } else{
+ pc.printf("MPU6050 test failed \n");
+ }
+
+ // wait for ready
+ pc.printf("\nSend any character to begin DMP programming and demo: ");
+
+ while(!pc.readable());
+ pc.getc();
+ pc.printf("\n");
+
+ // load and configure the DMP
+ pc.printf("Initializing DMP...\n");
+ devStatus = mpu.dmpInitialize();
+
+ // supply your own gyro offsets here, scaled for min sensitivity
+ mpu.setXGyroOffset(-61);
+ mpu.setYGyroOffset(-127);
+ mpu.setZGyroOffset(19);
+ mpu.setZAccelOffset(16282); // 1688 factory default for my test chip
+
+ // make sure it worked (returns 0 if so)
+ if (devStatus == 0) {
+ // turn on the DMP, now that it's ready
+ pc.printf("Enabling DMP...\n");
+ mpu.setDMPEnabled(true);
+
+ // enable Arduino interrupt detection
+ pc.printf("Enabling interrupt detection (Arduino external interrupt 0)...\n");
+// attachInterrupt(0, dmpDataReady, RISING);
+ mpuIntStatus = mpu.getIntStatus();
+
+ // set our DMP Ready flag so the main loop() function knows it's okay to use it
+ pc.printf("DMP ready! Waiting for first interrupt...\n");
+ dmpReady = true;
+
+ // get expected DMP packet size for later comparison
+ packetSize = mpu.dmpGetFIFOPacketSize();
+ } else {
+ // ERROR!
+ // 1 = initial memory load failed
+ // 2 = DMP configuration updates failed
+ // (if it's going to break, usually the code will be 1)
+ pc.printf("DMP Initialization failed (code ");
+ pc.printf("%u",devStatus);
+ pc.printf(")\n");
+ }
+
+
+// ================================================================
+// === MAIN PROGRAM LOOP ===
+// ================================================================
+
+while(1)
+{
+ // if programming failed, don't try to do anything
+ if (!dmpReady) continue;
+ myled2=0;
+
+ // wait for MPU interrupt or extra packet(s) available
+// while (!mpuInterrupt && fifoCount < packetSize) {
+// while (!mpuIntStatus && fifoCount < packetSize)
+ {
+ // other program behavior stuff here
+ // .
+ // .
+ // .
+ // if you are really paranoid you can frequently test in between other
+ // stuff to see if mpuInterrupt is true, and if so, "break;" from the
+ // while() loop to immediately process the MPU data
+ // .
+ // .
+ // .
+// fifoCount= mpu.getFIFOCount();
+// mpuIntStatus = mpu.getIntStatus();
+ }
+ wait_us(500);
+
+ // reset interrupt flag and get INT_STATUS byte
+ mpuInterrupt = false;
+ mpuIntStatus = mpu.getIntStatus();
+
+ // get current FIFO count
+ fifoCount = mpu.getFIFOCount();
+
+ // check for overflow (this should never happen unless our code is too inefficient)
+ if ((mpuIntStatus & 0x10) || fifoCount == 1024) {
+ // reset so we can continue cleanly
+ mpu.resetFIFO();
+ pc.printf("FIFO overflow!");
+
+ // otherwise, check for DMP data ready interrupt (this should happen frequently)
+ } else if (mpuIntStatus & 0x02) {
+ // wait for correct available data length, should be a VERY short wait
+ while (fifoCount < packetSize) fifoCount = mpu.getFIFOCount();
+
+ // read a packet from FIFO
+ mpu.getFIFOBytes(fifoBuffer, packetSize);
+
+ // track FIFO count here in case there is > 1 packet available
+ // (this lets us immediately read more without waiting for an interrupt)
+ fifoCount -= packetSize;
+
+#ifdef OUTPUT_READABLE_QUATERNION
+ // display quaternion values in easy matrix form: w x y z
+ mpu.dmpGetQuaternion(&q, fifoBuffer);
+ // pc.printf("quat\t");
+ pc.printf("%f",q.w);
+ pc.printf(",");
+ pc.printf("%f",q.x);
+ pc.printf(",");
+ pc.printf("%f",q.y);
+ pc.printf(",");
+ pc.printf("%f\n",q.z);
+#endif
+
+#ifdef OUTPUT_READABLE_EULER
+ // display Euler angles in degrees
+ mpu.dmpGetQuaternion(&q, fifoBuffer);
+ mpu.dmpGetEuler(euler, &q);
+ pc.printf("euler\t");
+ pc.printf("%f",euler[0] * 180/M_PI);
+ pc.printf("\t");
+ pc.printf("%f",euler[1] * 180/M_PI);
+ pc.printf("\t");
+ pc.printf("%f\n",euler[2] * 180/M_PI);
+#endif
+
+#ifdef OUTPUT_READABLE_YAWPITCHROLL
+ // display Euler angles in degrees
+ mpu.dmpGetQuaternion(&q, fifoBuffer);
+ mpu.dmpGetGravity(&gravity, &q);
+ mpu.dmpGetYawPitchRoll(ypr, &q, &gravity);
+ pc.printf("ypr\t");
+ pc.printf("%f3.2",ypr[0] * 180/M_PI);
+ pc.printf("\t");
+ pc.printf("%f3.2",ypr[1] * 180/M_PI);
+ pc.printf("\t");
+ pc.printf("%f3.2\n",ypr[2] * 180/M_PI);
+#endif
+
+#ifdef OUTPUT_READABLE_REALACCEL
+ // display real acceleration, adjusted to remove gravity
+ mpu.dmpGetQuaternion(&q, fifoBuffer);
+ mpu.dmpGetAccel(&aa, fifoBuffer);
+ mpu.dmpGetGravity(&gravity, &q);
+ mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+ pc.printf("areal\t");
+ pc.printf("%d",aaReal.x);
+ pc.printf("\t");
+ pc.printf("%d",aaReal.y);
+ pc.printf("\t");
+ pc.printf("%d\n",aaReal.z);
+#endif
+
+#ifdef OUTPUT_READABLE_WORLDACCEL
+ // display initial world-frame acceleration, adjusted to remove gravity
+ // and rotated based on known orientation from quaternion
+ mpu.dmpGetQuaternion(&q, fifoBuffer);
+ mpu.dmpGetAccel(&aa, fifoBuffer);
+ mpu.dmpGetGravity(&gravity, &q);
+ mpu.dmpGetLinearAccel(&aaReal, &aa, &gravity);
+ mpu.dmpGetLinearAccelInWorld(&aaWorld, &aaReal, &q);
+ pc.printf("aworld\t");
+ pc.printf("%d",aaWorld.x);
+ pc.printf("\t");
+ pc.printf("%d",aaWorld.y);
+ pc.printf("\t");
+ pc.printf("%d\n",aaWorld.z);
+#endif
+#ifdef OUTPUT_TEAPOT
+// display quaternion values in InvenSense Teapot demo format:
+ teapotPacket[2] = fifoBuffer[0];
+ teapotPacket[3] = fifoBuffer[1];
+ teapotPacket[4] = fifoBuffer[4];
+ teapotPacket[5] = fifoBuffer[5];
+ teapotPacket[6] = fifoBuffer[8];
+ teapotPacket[7] = fifoBuffer[9];
+ teapotPacket[8] = fifoBuffer[12];
+ teapotPacket[9] = fifoBuffer[13];
+
+ for(int i=0;i<14;i++)
+ {
+ pc.putc(teapotPacket[i]);
+ }
+// pc.printf("%d",teapotPacket, 14);
+ teapotPacket[11]++; // packetCount, loops at 0xFF on purpose
+#endif
+
+ // blink LED to indicate activity
+ blinkState = !blinkState;
+ myled1 = blinkState;
+ }
+ }
+}
\ No newline at end of file
diff -r 7d1bf3ce0053 -r 029b4b8111ef mbed.bld --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/mbed.bld Fri Jan 10 17:01:28 2014 +0000 @@ -0,0 +1,1 @@ +http://mbed.org/users/mbed_official/code/mbed/builds/dc225afb6914 \ No newline at end of file