uint8_t mpu6050_dmpInitialize() { // reset device DEBUG_PRINTLN("\r\nResetting MPU6050...\r\n"); mpu6050_reset(); Delay(30); // wait after reset // disable sleep mode DEBUG_PRINTLN("Disabling sleep mode...\r\n"); mpu6050_setSleepEnabled(false); // get MPU hardware revision DEBUG_PRINTLN("Selecting user bank 16...\r\n"); mpu6050_setMemoryBank(0x10, true, true); DEBUG_PRINTLN("Selecting memory byte 6...\r\n"); mpu6050_setMemoryStartAddress(0x06); DEBUG_PRINTLN("Checking hardware revision...\r\n"); uint8_t hwRevision = mpu6050_readMemoryByte(); DEBUG_PRINTLN("Revision @ user[16][6] = 0x%0x\r\n", hwRevision); DEBUG_PRINTLN("Resetting memory bank selection to 0...\r\n"); mpu6050_setMemoryBank(0, false, false); // check OTP bank valid DEBUG_PRINTLN("Reading OTP bank valid flag...\r\n"); uint8_t otpValid = mpu6050_getOTPBankValid(); DEBUG_PRINTLN("OTP bank is %s\r\n", otpValid ? "valid!" : "invalid!"); // get X/Y/Z gyro offsets DEBUG_PRINTLN("Reading gyro offset values..."); int8_t xgOffset = mpu6050_getXGyroOffsetTC(); int8_t ygOffset = mpu6050_getYGyroOffsetTC(); int8_t zgOffset = mpu6050_getZGyroOffsetTC(); DEBUG_PRINTLN("X gyro offset = %d\r\n", xgOffset); DEBUG_PRINTLN("Y gyro offset = %d\r\n", ygOffset); DEBUG_PRINTLN("Z gyro offset = %d\r\n", zgOffset); // i2c_read_byte(&mpu6050, MPU6050_RA_USER_CTRL, buffer); // ? DEBUG_PRINTLN("Enabling interrupt latch, clear on any read, AUX bypass enabled\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_INT_PIN_CFG, 0x32); /* hmc5883l setting */ // i2c_write_byte(&hmc5883l, HMC5883L_RA_CONFIG_A, 0x70); hmc5883l_initialize(); // load DMP code into memory banks DEBUG_PRINTLN("Writing DMP code to MPU memory banks\r\n"); if (mpu6050_writeProgMemoryBlock(dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, false)) { DEBUG_PRINTLN("Success! DMP code written and verified.\r\n"); DEBUG_PRINTLN("Configuring DMP and related settings...\r\n"); // write DMP configuration DEBUG_PRINT("Writing DMP configuration to MPU memory banks\r\n"); if (mpu6050_writeProgDMPConfigurationSet(dmpConfig, MPU6050_DMP_CONFIG_SIZE)) { DEBUG_PRINTLN("Success! DMP configuration written and verified.\r\n"); DEBUG_PRINTLN("Setting DMP and FIFO_OFLOW interrupts enabled...\r\n"); mpu6050_setIntEnabled(0x12); DEBUG_PRINTLN("Setting sample rate to 200Hz...\r\n"); mpu6050_setRate(4); // 1khz / (1 + 4) = 200 Hz DEBUG_PRINTLN("Setting clock source to Z Gyro...\r\n"); mpu6050_setClockSource(MPU6050_CLOCK_PLL_ZGYRO); DEBUG_PRINTLN("Setting DLPF bandwidth to 42Hz...\r\n"); mpu6050_setDLPFMode(MPU6050_DLPF_BW_42); DEBUG_PRINTLN("Setting external frame sync to TEMP_OUT_L[0]...\r\n"); mpu6050_setExternalFrameSync(MPU6050_EXT_SYNC_TEMP_OUT_L); DEBUG_PRINTLN("Setting gyro sensitivity to +/- 2000 deg/sec...\r\n"); mpu6050_setFullScaleGyroRange(MPU6050_GYRO_FS_2000); DEBUG_PRINTLN("Setting DMP configuration bytes (function unknown)...\r\n"); mpu6050_setDMPConfig1(0x03); mpu6050_setDMPConfig2(0x00); DEBUG_PRINTLN("Clearing OTP Bank flag...\r\n"); mpu6050_setOTPBankValid(false); DEBUG_PRINTLN("Setting X/Y/Z gyro offsets to previous values...\r\n"); mpu6050_setXGyroOffsetTC(xgOffset); mpu6050_setYGyroOffsetTC(ygOffset); mpu6050_setZGyroOffsetTC(zgOffset); DEBUG_PRINTLN("Setting X/Y/Z gyro user offsets to zero...\r\n"); /* mpu6050_setXGyroOffset(0); mpu6050_setYGyroOffset(0); mpu6050_setZGyroOffset(0); */ // mpu6050_setXGyroOffset(220); // mpu6050_setYGyroOffset(76); // mpu6050_setZGyroOffset(-85); // mpu6050_setZAccelOffset(700); DEBUG_PRINTLN("Writing final memory update 1/19 (function unknown)...\r\n"); uint8_t dmpUpdate[16], j; uint16_t pos = 0; for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 2/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Resetting FIFO...\r\n"); mpu6050_resetFIFO(); DEBUG_PRINTLN("Reading FIFO count...\r\n"); uint8_t fifoCount = mpu6050_getFIFOCount(); DEBUG_PRINT("Current FIFO count=\r\n"); DEBUG_PRINTLN(fifoCount); uint8_t fifoBuffer[128]; // mpu6050_getFIFOBytes(fifoBuffer, fifoCount); DEBUG_PRINTLN("Writing final memory update 3/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 4/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Disabling all standby flags...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_PWR_MGMT_2, 0x00); DEBUG_PRINTLN("Setting accelerometer sensitivity to +/- 2g...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_ACCEL_CONFIG, 0x00); DEBUG_PRINTLN("Setting motion detection threshold to 2...\r\n"); mpu6050_setMotionDetectionThreshold(2); DEBUG_PRINTLN("Setting zero-motion detection threshold to 156...\r\n"); mpu6050_setZeroMotionDetectionThreshold(156); DEBUG_PRINTLN("Setting motion detection duration to 80...\r\n"); mpu6050_setMotionDetectionDuration(80); DEBUG_PRINTLN("Setting zero-motion detection duration to 0...\r\n"); mpu6050_setZeroMotionDetectionDuration(0); DEBUG_PRINTLN("Setting AK8975 to single measurement mode...\r\n"); // to do //mag -> setMode(1); #if 1 /* set hmc5883l */ // i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_CTRL, 0x40); /* slave1 for read */ i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_ADDR, HMC5883L_ADDRESS | I2C_READ); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_REG, HMC5883L_RA_DATAX_H); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_CTRL, 0x86); /* slave1 for write */ i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_ADDR, HMC5883L_ADDRESS); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_REG, HMC5883L_RA_MODE); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_DO, HMC5883L_MODE_SINGLE); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_CTRL, 0x81); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV4_CTRL, 0x18); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); #endif #if 0 // setup AK8975 (0x0E) as Slave 0 in read mode DEBUG_PRINTLN("Setting up AK8975 read slave 0...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_ADDR, 0x1E); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_REG, 0x01); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV0_CTRL, 0xDA); // setup AK8975 (0x0E) as Slave 2 in write mode DEBUG_PRINTLN("Setting up AK8975 write slave 2...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_ADDR, 0x0E); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_REG, 0x0A); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_CTRL, 0x81); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV2_DO, 0x01); // setup I2C timing/delay control DEBUG_PRINTLN("Setting up slave access delay...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_SLV4_CTRL, 0x18); i2c_write_byte(&mpu6050, MPU6050_RA_I2C_MST_DELAY_CTRL, 0x05); #endif // enable interrupts DEBUG_PRINTLN("Enabling default interrupt behavior/no bypass...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_INT_PIN_CFG, 0x00); // enable I2C master mode and reset DMP/FIFO DEBUG_PRINTLN("Enabling I2C master mode...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x20); DEBUG_PRINTLN("Resetting FIFO...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x24); DEBUG_PRINTLN("Rewriting I2C master mode enabled because...I don't know\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0x20); DEBUG_PRINTLN("Enabling and resetting DMP/FIFO...\r\n"); i2c_write_byte(&mpu6050, MPU6050_RA_USER_CTRL, 0xE8); DEBUG_PRINTLN("Writing final memory update 5/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 6/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 7/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 8/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 9/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 10/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 11/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Reading final memory update 12/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); #ifdef DEBUG DEBUG_PRINT("Read bytes: "); for (j = 0; j < 4; j++) { DEBUG_PRINTF("0x%02x ", dmpUpdate[3 + j]); } DEBUG_PRINTLN("\r\n"); #endif DEBUG_PRINTLN("Writing final memory update 13/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 14/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 15/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 16/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Writing final memory update 17/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Waiting for FIFO count >= 46...\r\n"); while ((fifoCount = mpu6050_getFIFOCount()) < 46); DEBUG_PRINTLN("Reading FIFO...\r\n"); mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes DEBUG_PRINTLN("Reading interrupt status...\r\n"); mpu6050_getIntStatus(); DEBUG_PRINTLN("Writing final memory update 18/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Waiting for FIFO count >= 48...\r\n"); while ((fifoCount = mpu6050_getFIFOCount()) < 48); DEBUG_PRINTLN("Reading FIFO..."); mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes DEBUG_PRINTLN("Reading interrupt status...\r\n"); mpu6050_getIntStatus(); DEBUG_PRINTLN("Waiting for FIFO count >= 48...\r\n"); while ((fifoCount = mpu6050_getFIFOCount()) < 48); DEBUG_PRINTLN("Reading FIFO...\r\n"); mpu6050_getFIFOBytes(fifoBuffer, min(fifoCount, 128)); // safeguard only 128 bytes DEBUG_PRINTLN("Reading interrupt status...\r\n"); mpu6050_getIntStatus(); DEBUG_PRINTLN("Writing final memory update 19/19 (function unknown)...\r\n"); for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], true); DEBUG_PRINTLN("Disabling DMP (you turn it on later)...\r\n"); mpu6050_setDMPEnabled(false); DEBUG_PRINTLN("Setting up internal 48-byte (default) DMP packet buffer...\r\n"); dmpPacketSize = 48; /*if ((dmpPacketBuffer = (uint8_t *)malloc(42)) == 0) { return 3; // TODO: proper error code for no memory }*/ DEBUG_PRINTLN("Resetting FIFO and clearing INT status one last time...\r\n"); mpu6050_resetFIFO(); mpu6050_getIntStatus(); } else { DEBUG_PRINTLN("ERROR! DMP configuration verification failed.\r\n"); return 2; // configuration block loading failed } } else { DEBUG_PRINTLN("ERROR! DMP code verification failed.\r\n"); return 1; // main binary block loading failed } return 0; // success }
/* * initialize mpu6050 dmp */ uint8_t mpu6050_dmpInitialize() { //setup interrupt MPU6050_DMP_INT0SETUP; //reset mpu6050_writeBit(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_DEVICE_RESET_BIT, 1); _delay_ms(30);//wait after reset //disable sleep mode mpu6050_setSleepDisabled(); //set memorybank to 0 mpu6050_setMemoryBank(0, 0, 0); //get X/Y/Z gyro offsets int8_t xgOffset = mpu6050_getXGyroOffset(); int8_t ygOffset = mpu6050_getYGyroOffset(); int8_t zgOffset = mpu6050_getZGyroOffset(); //setting slave 0 address to 0x7F mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x7F); //disabling I2C Master mode mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_EN_BIT, 0); //setting slave 0 address to 0x68 (self) mpu6050_writeByte(MPU6050_RA_I2C_SLV0_ADDR + 0*3, 0x68); //resetting I2C Master control mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_I2C_MST_RESET_BIT, 1); _delay_ms(20); //load DMP code into memory banks if (mpu6050_writeMemoryBlock(mpu6050_dmpMemory, MPU6050_DMP_CODE_SIZE, 0, 0, 1, 1) == 1) { if (mpu6050_writeDMPConfigurationSet(mpu6050_dmpConfig, MPU6050_DMP_CONFIG_SIZE, 1)) { //set clock source mpu6050_writeBits(MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_ZGYRO); //set DMP and FIFO_OFLOW interrupts enabled mpu6050_writeByte(MPU6050_RA_INT_ENABLE, 0x12); //set sample rate mpu6050_writeByte(MPU6050_RA_SMPLRT_DIV, 4); // 1khz / (1 + 4) = 200 Hz //set external frame sync to TEMP_OUT_L[0] mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_EXT_SYNC_SET_BIT, MPU6050_CFG_EXT_SYNC_SET_LENGTH, MPU6050_EXT_SYNC_TEMP_OUT_L); //set DLPF bandwidth to 42Hz mpu6050_writeBits(MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); //set gyro sensitivity to +/- 2000 deg/sec mpu6050_writeBits(MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS_2000); //set DMP configuration bytes (function unknown) mpu6050_writeByte(MPU6050_RA_DMP_CFG_1, 0x03); mpu6050_writeByte(MPU6050_RA_DMP_CFG_2, 0x00); //clear OTP Bank flag mpu6050_writeBit(MPU6050_RA_XG_OFFS_TC, MPU6050_TC_OTP_BNK_VLD_BIT, 0); //set X/Y/Z gyro offsets to previous values //xgOffset = 0; //ygOffset = 0; zgOffset = 90; mpu6050_setXGyroOffset(xgOffset); mpu6050_setYGyroOffset(ygOffset); mpu6050_setZGyroOffset(zgOffset); //set X/Y/Z gyro user offsets to zero mpu6050_writeWords(MPU6050_RA_XG_OFFS_USRH, 1, 0); mpu6050_writeWords(MPU6050_RA_YG_OFFS_USRH, 1, 0); mpu6050_writeWords(MPU6050_RA_ZG_OFFS_USRH, 1, 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(&mpu6050_dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); //writing final memory update 2/7 (function unknown) for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); //reset FIFO mpu6050_writeBits(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1, 1); //reading FIFO count uint8_t fifoCount = mpu6050_getFIFOCount(); uint8_t fifoBuffer[128]; //current FIFO count mpu6050_readBytes(MPU6050_RA_FIFO_R_W, fifoCount, fifoBuffer); //setting motion detection threshold to 2 mpu6050_writeByte(MPU6050_RA_MOT_THR, 2); //setting zero-motion detection threshold to 156 mpu6050_writeByte(MPU6050_RA_ZRMOT_THR, 156); //setting motion detection duration to 80 mpu6050_writeByte(MPU6050_RA_MOT_DUR, 80); //setting zero-motion detection duration to 0 mpu6050_writeByte(MPU6050_RA_ZRMOT_DUR, 0); //reset FIFO mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); //enabling FIFO mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, 1); //enabling DMP mpu6050_dmpEnable(); //resetting DMP mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_DMP_RESET_BIT, 1); //waiting for FIFO count > 2 while ((fifoCount = mpu6050_getFIFOCount()) < 3); //writing final memory update 3/7 (function unknown) for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); //writing final memory update 4/7 (function unknown) for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); //writing final memory update 5/7 (function unknown) for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); //reading FIFO data...")); mpu6050_getFIFOBytes(fifoBuffer, fifoCount); //reading final memory update 6/7 (function unknown) for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); mpu6050_readMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1]); //waiting for FIFO count > 2 while ((fifoCount = mpu6050_getFIFOCount()) < 3); //reading FIFO data mpu6050_getFIFOBytes(fifoBuffer, fifoCount); //writing final memory update 7/7 (function unknown) for (j = 0; j < 4 || j < dmpUpdate[2] + 3; j++, pos++) dmpUpdate[j] = pgm_read_byte(&mpu6050_dmpUpdates[pos]); mpu6050_writeMemoryBlock(dmpUpdate + 3, dmpUpdate[2], dmpUpdate[0], dmpUpdate[1], 1, 0); //disabling DMP (you turn it on later) mpu6050_dmpDisable(); //resetting FIFO and clearing INT status one last time mpu6050_writeBit(MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_RESET_BIT, 1); } else { return 2; // configuration block loading failed } } else { return 1; // main binary block loading failed } return 0; // success }
/* * write a dmp configuration set */ unsigned char mpu6050_writeDMPConfigurationSet(char address, const unsigned char *data, unsigned short dataSize, unsigned char useProgMem) { unsigned char progBuffer[8]; unsigned char *pProgBuffer; pProgBuffer = &progBuffer[0]; unsigned char success, special; unsigned short i, j; //if (useProgMem) { // progBuffer = (unsigned char *)malloc(8); // assume 8-byte blocks, realloc later if necessary //} // config set data is a long string of blocks with the following structure: // [bank] [offset] [length] [byte[0], byte[1], ..., byte[length]] unsigned char bank, offset, length; for (i = 0; i < dataSize;) { if (useProgMem) { bank = data[i++]; offset = data[i++]; length = data[i++]; } else { bank = *data; data++; i++; offset = *data; data++; i++; length = *data; data++; i++; } // write data or perform special action if (length > 0) { // regular block of data to write if (useProgMem) { //if (sizeof(progBuffer) < length) progBuffer = (unsigned char *)realloc(progBuffer, length); for (j = 0; j < length; j++) progBuffer[j] = 1; //pgm_read_byte(data + i + j); } else { for (j = 0; j < length; j++) { progBuffer[j] = *data; data++; } } success = mpu6050_writeMemoryBlock(address, pProgBuffer, length, bank, offset, 0, 0); i += length; } else { // special instruction // NOTE: this kind of behavior (what and when to do certain things) // is totally undocumented. This code is in here based on observed // behavior only, and exactly why (or even whether) it has to be here // is anybody's guess for now. if (useProgMem) { special = 1; //pgm_read_byte(data + i++); } else { special = *data; data++; i++; } if (special == 0x01) { // enable DMP-related interrupts //write_bit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_ZMOT_BIT, 1); //setIntZeroMotionEnabled //write_bit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_FIFO_OFLOW_BIT, 1); //setIntFIFOBufferOverflowEnabled //write_bit(MPU6050_RA_INT_ENABLE, MPU6050_INTERRUPT_DMP_INT_BIT, 1); //setIntDMPEnabled unsigned char temp = 0x32; i2cWrite(address, MPU6050_INT_ENABLE, &temp, 1); // single operation success = 1; } else { // unknown special command success = 0; } } if (!success) { //if (useProgMem) free(progBuffer); return 0; // uh oh } } //if (useProgMem) free(progBuffer); return 1; }