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 Accelerometer::getYawPitchRoll(float *data) { uint8_t fifoBuffer[64]; Quaternion q; VectorFloat gravity; uint16_t packetSize = getFIFOPacketSize(); getFIFOBytes(fifoBuffer, packetSize); getQuaternion(&q, fifoBuffer); getGravity(&gravity, &q); getYawPitchRoll(data, &q, &gravity); data[0] = (data[0] * 180/M_PI) - zeroValues[0]; data[1] = (data[1] * 180/M_PI) - zeroValues[1]; data[2] = (data[2] * 180/M_PI) - zeroValues[2]; return 0; }
Accelerometer::Accelerometer() { m_i2cfd = open("/dev/i2c-1", O_RDWR); if(!m_i2cfd < 0) abort(); if(ioctl(m_i2cfd, I2C_SLAVE, 0x68) < 0) abort(); i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); // Set Clock source i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_250); // Full scale gyro range i2cWriteBits(MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS_2); // Full scale accel range i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1, true); // Reset usleep(30000); i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, 1, false); // Disable sleep mode setMemoryBank(0x10, true, true); // Get MPU hardware revision uint8_t value = 0x06; // Alright, this is dirty but a pointer to this value will be sent to every i2cWriteBytes... uint16_t value16; // Yeah... I know, I won't go to heaven but that's ok i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &value); // Set memory start address i2cReadBytes(MPU6050_RA_MEM_R_W, 1, buffer); // Get hardware revision uint8_t hwRevision __attribute__((__unused__)) = buffer[0]; setMemoryBank(0, false, false); i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 1, buffer); uint8_t otpValid __attribute__((__unused__)) = buffer[0]; // Check OTP bank validity i2cReadBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); int8_t xgOffset = buffer[0]; i2cReadBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); int8_t ygOffset = buffer[0]; i2cReadBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, buffer); int8_t zgOffset = buffer[0]; value = 0x7F; // Setup weird slave stuff i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value); i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 1, false); // Disable I2C Master Mode value = 0x68; i2cWriteBytes(MPU6050_RA_I2C_SLV0_ADDR, 1, &value); i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1, true); // Reset I2C Master usleep(20000); if (writeMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, true, true)) { // Load DMP code into memory banks if (writeDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE, true)) { // Let's write the DMP configuration i2cWriteBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); // Set clock source value = 0x12; i2cWriteBytes(MPU6050_RA_INT_ENABLE, 1, &value); // Enable 0x12 interuption value = 4; // 1khz / (1 + 4) = 200 Hz i2cWriteBytes(MPU6050_RA_SMPLRT_DIV, 1, &value); i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); // External Frame Sync i2cWriteBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); // DLPF Mode i2cWriteBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); // Full scale gyro range value = 0x03; i2cWriteBytes(MPU6050_RA_DMP_CFG_1, 1, &value); value = 0x00; i2cWriteBytes(MPU6050_RA_DMP_CFG_2, 1, &value); // Setting X Y and Z gyro offsets to previous values i2cWriteBits(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, xgOffset); i2cWriteBits(MPU6050_RA_YG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, ygOffset); i2cWriteBits(MPU6050_RA_ZG_OFFS_TC, MPU6050_TC_OFFSET_BIT, MPU6050_TC_OFFSET_LENGTH, zgOffset); value16 = 0; // And gyro offsets users to 0 i2cWriteWords(MPU6050_RA_XG_OFFS_USRH, 1, &value16); i2cWriteWords(MPU6050_RA_YG_OFFS_USRH, 1, &value16); i2cWriteWords(MPU6050_RA_ZG_OFFS_USRH, 1, &value16); // Memory update 1/7 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]); //Memory update 2/7 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]); resetFIFO(); uint8_t fifoCount = getFIFOCount(); uint8_t fifoBuffer[128]; value = 2; i2cWriteBytes(MPU6050_RA_MOT_THR, 1, &value); // Motion threshold value = 156; i2cWriteBytes(MPU6050_RA_ZRMOT_THR, 1, &value); // Zero Motion threshold value = 80; i2cWriteBytes(MPU6050_RA_MOT_DUR, 1, &value); // Motion duration value = 0; i2cWriteBytes(MPU6050_RA_ZRMOT_DUR, 1, &value); // Zero Motion duration resetFIFO(); setFIFOEnabled(true); setDMPEnabled(true); i2cWriteBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1, true); // Reset DMP // Memory update 3/7 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]); // Memory update 4/7 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]); // Memory update 5/7 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]); while ((fifoCount = getFIFOCount()) < 3); getFIFOBytes(fifoBuffer, fifoCount); uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus(); // Memory update 6/7 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]); while ((fifoCount = getFIFOCount()) < 3); getFIFOBytes(fifoBuffer, fifoCount); mpuIntStatus = getIntStatus(); // Memory update 7/7 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]); setDMPEnabled(false); dmpPacketSize = 42; resetFIFO(); getIntStatus(); } else abort(); } else abort(); i2cReadBits(MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buffer); if(buffer[0] != 0x34) abort(); setDMPEnabled(true); zeroValues[0] = 0; zeroValues[1] = 0; zeroValues[2] = 0; }
unsigned char dmpInitialize() { // reset device mpu_reset(); DELAY_US(30*1000); // disable sleep mode setSleepEnabled(false); // get MPU hardware revision setMemoryBank(0x10, true, true); setMemoryStartAddress(0x06); unsigned char hwRevision = readMemoryByte(); setMemoryBank(0, false, false); // check OTP bank valid unsigned char otpValid = getOTPBankValid(); // get X/Y/Z gyro offsets char xgOffsetTC = getXGyroOffset(); char ygOffsetTC = getYGyroOffset(); char zgOffsetTC = getZGyroOffset(); // setup weird slave stuff (?) // setSlaveAddress(0, 0x7F); // setI2CMasterModeEnabled(false); // setSlaveAddress(0, 0x68); // resetI2CMaster(); DELAY_US(20*1000); // load DMP code into memory banks if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE,0,0,false)) { if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { setClockSource(MPU6050_CLOCK_PLL_ZGYRO); setIntEnabled(0x12); setRate(4); // 1khz / (1 + 4) = 200 Hz setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); setDLPFMode(MPU6050_DLPF_BW_42); // setFullScaleGyroRange(MPU6050_GYRO_FS_2000); // setFullScaleAccelRange(MPU6050_ACCEL_FS_16); setDMPConfig1(0x03); setDMPConfig2(0x00); setOTPBankValid(false); // setXGyroOffset(xgOffsetTC); // setYGyroOffset(ygOffsetTC); // setZGyroOffset(zgOffsetTC); unsigned char dmpUpdate[16], j; unsigned int 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], true, false); 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], true, false); resetFIFO(); unsigned int fifoCount = getFIFOCount(); if (fifoCount > 128) { fifoCount = 128; } getFIFOBytes(fifoBuffer, fifoCount); setMotionDetectionThreshold(2); setZeroMotionDetectionThreshold(156); setMotionDetectionDuration(80); setZeroMotionDetectionDuration(0); resetFIFO(); setFIFOEnabled(true); setDMPEnabled(true); resetDMP(); 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], true, false); 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], true, false); 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], true, false); // while ((getFIFOCount()) < 3) {DELAY_US(500);} // // fifoCount = getFIFOCount(); // // getFIFOBytes(fifoBuffer, fifoCount); unsigned char mpuIntStatus = getIntStatus(); 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]); // while ((getFIFOCount()) < 3) {DELAY_US(500);} // // fifoCount = getFIFOCount(); // getFIFOBytes(fifoBuffer, fifoCount); mpuIntStatus = getIntStatus(); 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], true, false); setDMPEnabled(true); resetFIFO(); getIntStatus(); } else { return 2; // configuration block loading failed } } else { return 1; // main binary block loading failed } return 0; // success }
/** * Initialize DMP inside MPU6050 * @return boolean */ uint8_t MPU6050::dmpInitialize() { //Resetting MPU6050... reset(); usleep(30000); // wait after reset //Disabling sleep mode... setSleepEnabled(false); // get MPU hardware revision //Selecting user bank 16... setMemoryBank(0x10, true, true); //Selecting memory byte 6... setMemoryStartAddress(0x06); //Checking hardware revision... uint8_t hwRevision __attribute__((__unused__)) = readMemoryByte(); //Revision @ user[16][6] = //hwRevision, HEX); //Resetting memory bank selection to 0... setMemoryBank(0, false, false); // check OTP bank valid //Reading OTP bank valid flag... uint8_t otpValid __attribute__((__unused__)) = getOTPBankValid(); //OTP bank is //otpValid ? F("valid!") : F("invalid! // get X/Y/Z gyro offsets //Reading gyro offset values... /* int8_t xgOffset = getXGyroOffset(); int8_t ygOffset = getYGyroOffset(); int8_t zgOffset = getZGyroOffset(); sleep(5); for(int cnt = 0; cnt < 1000; cnt = cnt + 1) { std::cout << "X: " << typeid(xgOffset).name() << "Y: " << typeid(ygOffset).name() << "Z: " << typeid(zgOffset).name() << "\n"; sleep(1); //xgOffset = (getXGyroOffset() + xgOffset)/2; //ygOffset = (getYGyroOffset() + ygOffset)/2; //zgOffset = (getZGyroOffset() + zgOffset)/2; } */ //X gyro offset = //xgOffset); //Y gyro offset = //ygOffset); //Z gyro offset = //zgOffset); // setup weird slave stuff (?) //Setting slave 0 address to 0x7F... setSlaveAddress(0, 0x7F); //Disabling I2C Master mode... setI2CMasterModeEnabled(false); //Setting slave 0 address to 0x68 (self)... setSlaveAddress(0, 0x68); /* if (addr == 104) { std::cout << "Address: 104"; setSlaveAddress(0, 0x68); } else { std::cout << "Address: 105"; setSlaveAddress(0, 0x69); } * */ //Resetting I2C Master control... resetI2CMaster(); usleep(20000); // load DMP code into memory banks //Writing DMP code to MPU memory banks ( // bytes) if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { // write DMP configuration //Writing DMP configuration to MPU memory banks ( // bytes in config def) if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { //Setting clock source to Z Gyro... setClockSource(MPU6050_CLOCK_PLL_ZGYRO); //Setting DMP and FIFO_OFLOW interrupts enabled... setIntEnabled(0x12); //Setting sample rate to 200Hz... setRate(4); // 1khz / (1 + 4) = 200 Hz //Setting external frame sync to TEMP_OUT_L[0]... setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); //Setting DLPF bandwidth to 42Hz... setDLPFMode(MPU6050_DLPF_BW_42); //Setting gyro sensitivity to +/- 2000 deg/sec... setFullScaleGyroRange(MPU6050_GYRO_FS_2000); //Setting DMP configuration bytes (function unknown)... setDMPConfig1(0x03); setDMPConfig2(0x00); //Clearing OTP Bank flag... setOTPBankValid(false); /* //Setting X/Y/Z gyro offsets to previous values... setXGyroOffset(xgOffset); setYGyroOffset(ygOffset); setZGyroOffset(zgOffset); //Setting X/Y/Z gyro user offsets to zero... setXGyroOffsetUser(0); setYGyroOffsetUser(0); setZGyroOffsetUser(0); */ //Writing final memory update 1/7 (function unknown)... 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]); //Writing final memory update 2/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]); //Resetting FIFO... resetFIFO(); //Reading FIFO count... uint8_t fifoCount = getFIFOCount(); uint8_t fifoBuffer[128]; //fifoCount); if (fifoCount > 0) getFIFOBytes(fifoBuffer, fifoCount); //Setting motion detection threshold to 2... setMotionDetectionThreshold(2); //Setting zero-motion detection threshold to 156... setZeroMotionDetectionThreshold(156); //Setting motion detection duration to 80... setMotionDetectionDuration(80); //Setting zero-motion detection duration to 0... setZeroMotionDetectionDuration(0); //Resetting FIFO... resetFIFO(); //Enabling FIFO... setFIFOEnabled(true); //Enabling DMP... setDMPEnabled(true); //Resetting DMP... resetDMP(); //Writing final memory update 3/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]); //Writing final memory update 4/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]); //Writing final memory update 5/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]); while ((fifoCount = getFIFOCount()) < 3); //Reading FIFO data... getFIFOBytes(fifoBuffer, fifoCount); //Reading interrupt status... uint8_t mpuIntStatus __attribute__((__unused__)) = getIntStatus(); //Current interrupt status= mpuIntStatus, HEX //Reading final memory update 6/7 (function unknown)... for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); //Waiting for FIFO count > 2... while ((fifoCount = getFIFOCount()) < 3); //Current FIFO count= //fifoCount); //Reading FIFO data... getFIFOBytes(fifoBuffer, fifoCount); //Reading interrupt status... mpuIntStatus = getIntStatus(); //Current interrupt status= //mpuIntStatus, HEX //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]); //DMP is good to go! Finally. //Disabling DMP (you turn it on later)... setDMPEnabled(false); //Setting up internal 42-byte (default) DMP packet buffer... dmpPacketSize = 42; /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { return 3; // TODO: proper error code for no memory }*/ //Resetting FIFO and clearing INT status one last time... resetFIFO(); getIntStatus(); } else { //ERROR! DMP configuration verification failed. return 2; // configuration block loading failed } } else { //ERROR! DMP code verification failed. return 1; // main binary block loading failed } return 0; // success }
float MPU6050::dmpGetFirstYPRData() { long long time = timeCheck(); if(dmpReady && time > 0) { // get current FIFO count dmpFifoCount = getFIFOCount(); if (dmpFifoCount == 1024) { // reset so we can continue cleanly resetFIFO(); timeReset(); // otherwise, check for DMP data ready interrupt (this should happen frequently) } else if (dmpFifoCount >= 42) { timeCount(time); // read a packet from FIFO getFIFOBytes(dmpFifoBuffer, dmpPacketSize); // display quaternion values in easy matrix form: w x y z dmpGetQuaternion(&dmpQuat, dmpFifoBuffer); // if(dmpDebug) printf("quat %7.2f %7.2f %7.2f %7.2f ", dmpQuat.w,dmpQuat.x,dmpQuat.y,dmpQuat.z); #ifdef OUTPUT_READABLE_EULER // display Euler angles in degrees dmpGetEuler(dmpEuler, &dmpQuat); dmpEuler[0] = dmpEuler[0] * 180/M_PI; dmpEuler[1] = dmpEuler[1] * 180/M_PI; dmpEuler[2] = dmpEuler[2] * 180/M_PI; // if(dmpDebug) printf("euler %7.2f %7.2f %7.2f ", dmpEuler[0], dmpEuler[1], dmpEuler[2]); #endif #ifdef OUTPUT_READABLE_YAWPITCHROLL // display Euler angles in degrees dmpGetGravity(&dmpGravity, &dmpQuat); dmpGetYawPitchRoll(dmpYawPitchRoll, &dmpQuat, &dmpGravity); dmpYawPitchRoll[0] = dmpYawPitchRoll[0] * 180/M_PI; dmpYawPitchRoll[1] = dmpYawPitchRoll[1] * 180/M_PI; dmpYawPitchRoll[2] = dmpYawPitchRoll[2] * 180/M_PI; // if(dmpDebug) printf("ypr %7.2f %7.2f %7.2f ", dmpYawPitchRoll[0], dmpYawPitchRoll[1], dmpYawPitchRoll[2]); if(dmpDebug) return dmpYawPitchRoll[0]; #endif #ifdef OUTPUT_READABLE_REALACCEL // display real acceleration, adjusted to remove gravity dmpGetAccel(&dmpAccel, dmpFifoBuffer); dmpGetGravity(&dmpGravity, &dmpQuat); dmpGetLinearAccel(&dmpAccelReal, &dmpAccel, &dmpGravity); // if(dmpDebug) printf("areal %6d %6d %6d ", dmpAccelReal.x, dmpAccelReal.y, dmpAccelReal.z); #endif #ifdef OUTPUT_READABLE_WORLDACCEL // display initial world-frame acceleration, adjusted to remove gravity // and rotated based on known orientation from quaternion dmpGetAccel(&dmpAccel, dmpFifoBuffer); dmpGetGravity(&dmpGravity, &dmpQuat); dmpGetLinearAccelInWorld(&dmpAccelWorld, &dmpAccelReal, &dmpQuat); // if(dmpDebug) printf("aworld %6d %6d %6d ", dmpAccelWorld.x, dmpAccelWorld.y, dmpAccelWorld.z); #endif #ifdef OUTPUT_TEAPOT // display quaternion values in InvenSense Teapot demo format: dmpTeapotPacket[2] = dmpFifoBuffer[0]; dmpTeapotPacket[3] = dmpFifoBuffer[1]; dmpTeapotPacket[4] = dmpFifoBuffer[4]; dmpTeapotPacket[5] = dmpFifoBuffer[5]; dmpTeapotPacket[6] = dmpFifoBuffer[8]; dmpTeapotPacket[7] = dmpFifoBuffer[9]; dmpTeapotPacket[8] = dmpFifoBuffer[12]; dmpTeapotPacket[9] = dmpFifoBuffer[13]; //Serial.write(dmpTeapotPacket, 14); dmpTeapotPacket[11]++; // packetCount, loops at 0xFF on purpose #endif return -255; } } return -255; }
uint8_t MPU6050DMP::dmpInitialize() { // reset device DEBUG_PRINTLN(F("\n\nResetting MPU6050...")); reset(); delay(30); // wait after reset // enable sleep mode and wake cycle /*Serial.println(F("Enabling sleep mode...")); setSleepEnabled(true); Serial.println(F("Enabling wake cycle...")); setWakeCycleEnabled(true);*/ // disable sleep mode DEBUG_PRINTLN(F("Disabling sleep mode...")); setSleepEnabled(false); // get MPU hardware revision DEBUG_PRINTLN(F("Selecting user bank 16...")); setMemoryBank(0x10, true, true); DEBUG_PRINTLN(F("Selecting memory byte 6...")); setMemoryStartAddress(0x06); DEBUG_PRINTLN(F("Checking hardware revision...")); uint8_t hwRevision = readMemoryByte(); DEBUG_PRINT(F("Revision @ user[16][6] = ")); DEBUG_PRINTLNF(hwRevision, HEX); DEBUG_PRINTLN(F("Resetting memory bank selection to 0...")); setMemoryBank(0, false, false); // check OTP bank valid DEBUG_PRINTLN(F("Reading OTP bank valid flag...")); uint8_t otpValid = getOTPBankValid(); DEBUG_PRINT(F("OTP bank is ")); DEBUG_PRINTLN(otpValid ? F("valid!") : F("invalid!")); // 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); // 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); // load DMP code into memory banks DEBUG_PRINT(F("Writing DMP code to MPU memory banks (")); DEBUG_PRINT(MPU6050_DMP_CODE_SIZE); DEBUG_PRINTLN(F(" bytes)")); if (writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE)) { DEBUG_PRINTLN(F("Success! DMP code written and verified.")); // write DMP configuration DEBUG_PRINT(F("Writing DMP configuration to MPU memory banks (")); DEBUG_PRINT(MPU6050_DMP_CONFIG_SIZE); DEBUG_PRINTLN(F(" bytes in config def)")); if (writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { DEBUG_PRINTLN(F("Success! DMP configuration written and verified.")); DEBUG_PRINTLN(F("Setting clock source to Z Gyro...")); setClockSource(MPU6050_CLOCK_PLL_ZGYRO); DEBUG_PRINTLN(F("Setting DMP and FIFO_OFLOW interrupts enabled...")); setIntEnabled(0x12); DEBUG_PRINTLN(F("Setting sample rate to 200Hz...")); setRate(4); // 1khz / (1 + 4) = 200 Hz DEBUG_PRINTLN(F("Setting external frame sync to TEMP_OUT_L[0]...")); setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); DEBUG_PRINTLN(F("Setting DLPF bandwidth to 42Hz...")); setDLPFMode(MPU6050_DLPF_BW_42); DEBUG_PRINTLN(F("Setting gyro sensitivity to +/- 2000 deg/sec...")); setFullScaleGyroRange(MPU6050_GYRO_FS_2000); DEBUG_PRINTLN(F("Setting DMP configuration bytes (function unknown)...")); setDMPConfig1(0x03); setDMPConfig2(0x00); DEBUG_PRINTLN(F("Clearing OTP Bank flag...")); setOTPBankValid(false); DEBUG_PRINTLN(F("Setting X/Y/Z gyro offset TCs to previous values...")); setXGyroOffsetTC(xgOffsetTC); setYGyroOffsetTC(ygOffsetTC); setZGyroOffsetTC(zgOffsetTC); //DEBUG_PRINTLN(F("Setting X/Y/Z gyro user offsets to zero...")); //setXGyroOffset(0); //setYGyroOffset(0); //setZGyroOffset(0); DEBUG_PRINTLN(F("Writing final memory update 1/7 (function unknown)...")); 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)...")); 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...")); resetFIFO(); DEBUG_PRINTLN(F("Reading FIFO count...")); uint16_t fifoCount = getFIFOCount(); uint8_t fifoBuffer[128]; DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Setting motion detection threshold to 2...")); setMotionDetectionThreshold(2); DEBUG_PRINTLN(F("Setting zero-motion detection threshold to 156...")); setZeroMotionDetectionThreshold(156); DEBUG_PRINTLN(F("Setting motion detection duration to 80...")); setMotionDetectionDuration(80); DEBUG_PRINTLN(F("Setting zero-motion detection duration to 0...")); setZeroMotionDetectionDuration(0); DEBUG_PRINTLN(F("Resetting FIFO...")); resetFIFO(); DEBUG_PRINTLN(F("Enabling FIFO...")); setFIFOEnabled(true); DEBUG_PRINTLN(F("Enabling DMP...")); setDMPEnabled(true); DEBUG_PRINTLN(F("Resetting DMP...")); resetDMP(); DEBUG_PRINTLN(F("Writing final memory update 3/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("Writing final memory update 4/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("Writing final memory update 5/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("Waiting for FIFO count > 2...")); while ((fifoCount = getFIFOCount()) < 3); DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); DEBUG_PRINTLN(F("Reading FIFO data...")); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Reading interrupt status...")); uint8_t mpuIntStatus = getIntStatus(); DEBUG_PRINT(F("Current interrupt status=")); DEBUG_PRINTLNF(mpuIntStatus, HEX); DEBUG_PRINTLN(F("Reading final memory update 6/7 (function unknown)...")); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); DEBUG_PRINTLN(F("Waiting for FIFO count > 2...")); while ((fifoCount = getFIFOCount()) < 3); DEBUG_PRINT(F("Current FIFO count=")); DEBUG_PRINTLN(fifoCount); DEBUG_PRINTLN(F("Reading FIFO data...")); getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN(F("Reading interrupt status...")); mpuIntStatus = getIntStatus(); DEBUG_PRINT(F("Current interrupt status=")); DEBUG_PRINTLNF(mpuIntStatus, HEX); DEBUG_PRINTLN(F("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_PRINTLN(F("Disabling DMP (you turn it on later)...")); setDMPEnabled(false); DEBUG_PRINTLN(F("Setting up internal 42-byte (default) DMP packet buffer...")); dmpPacketSize = 42; /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { return 3; // TODO: proper error code for no memory }*/ DEBUG_PRINTLN(F("Resetting FIFO and clearing INT status one last time...")); resetFIFO(); getIntStatus(); } else { DEBUG_PRINTLN(F("ERROR! DMP configuration verification failed.")); return 2; // configuration block loading failed } } else { DEBUG_PRINTLN(F("ERROR! DMP code verification failed.")); return 1; // main binary block loading failed } return 0; // success }