void mpu6050_init(char address) { #if MPU6050_I2CINIT == 1 //init i2c i2c_init(); delay_us(10); #endif //set clock source // it is highly recommended that the device be configured to use one of the gyroscopes (or an external clock source) // as the clock reference for improved stability i2cWriteBits(address, MPU6050_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, MPU6050_CLOCK_PLL_XGYRO); //set DLPF bandwidth to 42Hz i2cWriteBits(address, MPU6050_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, MPU6050_DLPF_BW_42); //Sets sample rate to 8000/1+7 = 1000Hz unsigned char divider = SMPLRT_DIV; i2cWrite(address, MPU6050_SMPLRT_DIV, ÷r, 1); //50Hz //set gyro range i2cWriteBits(address, MPU6050_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, MPU6050_GYRO_FS); //set accel range i2cWriteBits(address, MPU6050_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, MPU6050_ACCEL_FS); //set sleep disabled mpu6050_setSleepDisabled(address); }
/* * 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 }