mohamed askar
/
MPU6050-DMP_6DOF
mpu6050 dmp test
Fork of MPU6050-DMP_6DOF by
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);