void Accelerometer::readMemoryBlock(uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address)
{
setMemoryBank(bank);
i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
uint8_t chunkSize;
for (uint16_t i = 0; i < dataSize;) {
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
if (i + chunkSize > dataSize)
chunkSize = dataSize - i;
if (chunkSize > 256 - address)
chunkSize = 256 - address;
i2cReadBytes(MPU6050_RA_MEM_R_W, chunkSize, data + i);
i += chunkSize;
address += chunkSize;
if (i < dataSize) {
if (address == 0)
bank++;
setMemoryBank(bank);
i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set Memory start address
}
}
}
bool Accelerometer::writeMemoryBlock(const uint8_t *data, uint16_t dataSize, uint8_t bank, uint8_t address, bool verify, bool useProgMem)
{
setMemoryBank(bank);
i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set Memory start address
uint8_t chunkSize;
uint8_t *verifyBuffer = NULL;
uint8_t *progBuffer = NULL;
uint16_t i;
uint8_t j;
if (verify)
verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
if (useProgMem)
progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
for (i = 0; i < dataSize;) {
chunkSize = MPU6050_DMP_MEMORY_CHUNK_SIZE;
if (i + chunkSize > dataSize)
chunkSize = dataSize - i;
if (chunkSize > 256 - address)
chunkSize = 256 - address;
if (useProgMem)
for (j = 0; j < chunkSize; j++)
progBuffer[j] = pgm_read_byte(data + i + j);
else
progBuffer = (uint8_t *)data + i;
i2cWriteBytes(MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
if (verify && verifyBuffer) {
setMemoryBank(bank);
i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
i2cReadBytes(MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer);
if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
free(verifyBuffer);
if (useProgMem)
free(progBuffer);
return false;
}
}
i += chunkSize;
address += chunkSize;
if (i < dataSize) {
if (address == 0)
bank++;
setMemoryBank(bank);
i2cWriteBytes(MPU6050_RA_MEM_START_ADDR, 1, &address); // Set memory start address
}
}
if (verify)
free(verifyBuffer);
if (useProgMem)
free(progBuffer);
return true;
}
void Accelerometer::setMemoryBank(uint8_t bank, bool prefetchEnabled, bool userBank)
{
bank &= 0x1F;
if (userBank)
bank |= 0x20;
if (prefetchEnabled)
bank |= 0x40;
i2cWriteBytes(MPU6050_RA_BANK_SEL, 1, &bank);
}
bool Accelerometer::writeDMPConfigurationSet(const uint8_t *data, uint16_t dataSize, bool useProgMem)
{
uint8_t *progBuffer = NULL, success, special;
uint16_t i, j;
if (useProgMem)
progBuffer = (uint8_t *)malloc(8);
uint8_t bank, offset, length;
for (i = 0; i < dataSize;) {
if (useProgMem) {
bank = pgm_read_byte(data + i++);
offset = pgm_read_byte(data + i++);
length = pgm_read_byte(data + i++);
} else {
bank = data[i++];
offset = data[i++];
length = data[i++];
}
if (length > 0) {
if (useProgMem) {
if (sizeof(progBuffer) < length)
progBuffer = (uint8_t *)realloc(progBuffer, length);
for (j = 0; j < length; j++)
progBuffer[j] = pgm_read_byte(data + i + j);
} else
progBuffer = (uint8_t *)data + i;
success = writeMemoryBlock(progBuffer, length, bank, offset, true);
i += length;
} else {
if (useProgMem)
special = pgm_read_byte(data + i++);
else
special = data[i++];
if (special == 0x01) {
uint8_t value = 0x32;
i2cWriteBytes(MPU6050_RA_INT_ENABLE, 1, &value);
success = true;
} else
success = false;
}
if (!success) {
if (useProgMem)
free(progBuffer);
return false;
}
}
if (useProgMem)
free(progBuffer);
return true;
}
int main(int argv,char *argc[])
{
unsigned char byte;
unsigned int word;
unsigned char block[32];
initAI();
block[0] = 0x00;
printf("%d\n",i2cWriteBytes(1,0x48,0x40,block,1));
//printf("0x%02x\n",getAI(AI_01));
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;
}