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;
}
/*
* write 1 byte to chip register
*/
void mpu6050_writeByte(uint8_t regAddr, uint8_t data) {
return mpu6050_writeBytes(regAddr, 1, &data);
}
