Rolf Ziegler
Complete MPU6050dmp example working with processing 2.1 teapot code v1.0 Jan. 2014
Fork of
MPU6050
by 
Shundo Kishi
Diff: MPU6050_6Axis_MotionApps20.h
- Revision:
- 6:029b4b8111ef
- Parent:
- 5:7d1bf3ce0053
--- 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);