bool mpu6050_getFIFOBytes(uint8_t *fifoBuffer, uint16_t fifoCount) {
if (fifoCount == 0) {
return true;
}
return mpu6050_readBytes(MPU6050_FIFO_R_W_REG_ADDR, fifoBuffer, fifoCount);
}
bool mpu6050_getFIFOCount(uint16_t *fifoCount) {
bool success;
uint8_t data[2];
success = mpu6050_readBytes(MPU6050_FIFO_COUNTH_REG_ADDR, data, 2);
*fifoCount = (((uint16_t)data[0]) << 8 | (uint16_t)data[1]);
return success;
}
bool mpu6050_writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify) {
bool success = true;
uint8_t chunkSize;
uint16_t i;
uint8_t progBuffer[1+MPU6050_DMP_MEMORY_CHUNK_SIZE];
uint8_t verifyBuffer[MPU6050_DMP_MEMORY_CHUNK_SIZE];
success &= mpu6050_setMemoryBank(bank, false, false);
success &= mpu6050_setMemoryStartAddress(address);
for (i = 0; i < dataSize;) {
// determine correct chunk size according to bank position and data size
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
// make sure we don't go past the data size
if (i + chunkSize > dataSize) {
chunkSize = dataSize - i;
}
// make sure this chunk doesn't go past the bank boundary (256 bytes)
if (chunkSize > 256 - address) {
chunkSize = 256 - address;
}
progBuffer[0] = MPU6050_MEM_R_W_REG_ADDR;
memcpy(&progBuffer[1], data + i, chunkSize);
mpu6050_writeBytes(progBuffer, chunkSize + 1);
if (verify) {
success &= mpu6050_setMemoryBank(bank, false, false);
success &= mpu6050_setMemoryStartAddress(address);
success &= mpu6050_readBytes(MPU6050_MEM_R_W_REG_ADDR, verifyBuffer, chunkSize);
if (memcmp(&progBuffer[1], verifyBuffer, chunkSize) != 0) {
success = false;
}
}
// increase byte index by [chunkSize]
i += chunkSize;
// uint8_t automatically wraps to 0 at 256
address += chunkSize;
// if we aren't done, update bank (if necessary) and address
if (i < dataSize) {
if (address == 0) {
bank++;
}
success &= mpu6050_setMemoryBank(bank, false, false);
success &= mpu6050_setMemoryStartAddress(address);
}
}
return success;
}
bool mpu6050_readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address) {
bool success = true;
uint8_t chunkSize;
uint16_t i;
mpu6050_setMemoryBank(bank, false, false);
mpu6050_setMemoryStartAddress(address);
for (i = 0; i < dataSize;) {
// determine correct chunk size according to bank position and data size
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
// make sure we don't go past the data size
if (i + chunkSize > dataSize) {
chunkSize = dataSize - i;
}
// make sure this chunk doesn't go past the bank boundary (256 bytes)
if (chunkSize > 256 - address) {
chunkSize = 256 - address;
}
// read the chunk of data as specified
success &= mpu6050_readBytes(MPU6050_MEM_R_W_REG_ADDR, data + i, chunkSize);
// increase byte index by [chunkSize]
i += chunkSize;
// uint8_t automatically wraps to 0 at 256
address += chunkSize;
// if we aren't done, update bank (if necessary) and address
if (i < dataSize) {
if (address == 0) {
bank++;
}
success &= mpu6050_setMemoryBank(bank, false, false);
success &= mpu6050_setMemoryStartAddress(address);
}
}
return 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
}
/*
* read 1 byte from chip register
*/
int8_t mpu6050_readByte(uint8_t regAddr, uint8_t *data) {
return mpu6050_readBytes(regAddr, 1, data);
}