/*
* read a memory block
*/
void mpu6050_readMemoryBlock(char address, unsigned char *data, unsigned short dataSize, unsigned char bank, unsigned char memAddress) {
mpu6050_setMemoryBank(address, bank, 0, 0);
mpu6050_setMemoryStartAddress(address, memAddress);
unsigned char chunkSize;
unsigned short i = 0;
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
i2cRead(address, MPU6050_MEM_R_W, data + i, chunkSize);
// increase byte index by [chunkSize]
i += chunkSize;
// unsigned char 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++;
mpu6050_setMemoryBank(address, bank, 0, 0);
mpu6050_setMemoryStartAddress(address, memAddress);
}
}
}
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;
}
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
}
/*
* write a memory block
*/
unsigned char mpu6050_writeMemoryBlock(char address, const unsigned char *data, unsigned short dataSize, unsigned char bank, unsigned char memAddress, unsigned char verify, unsigned char useProgMem) {
mpu6050_setMemoryBank(address, bank, 0, 0);
mpu6050_setMemoryStartAddress(address, memAddress);
unsigned char chunkSize;
//unsigned char *verifyBuffer = 0;
unsigned char progBuffer[MPU6050_DMP_MEMORY_CHUNK_SIZE];
unsigned char *pProgBuffer;
pProgBuffer = &progBuffer[0];
unsigned short i;
unsigned char j;
//if (verify) verifyBuffer = (unsigned char *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
//if (useProgMem) progBuffer = (unsigned char *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
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 - memAddress) chunkSize = 256 - memAddress;
if (useProgMem) {
// write the chunk of data as specified
for (j = 0; j < chunkSize; j++) progBuffer[j] = 1; //pgm_read_byte(data + i + j);
} else {
// write the chunck of data as specified
for (j = 0; j < chunkSize; j++) {
progBuffer[j] = *data;
data++;
}
}
i2cWrite(address, MPU6050_MEM_R_W, pProgBuffer, chunkSize);
/* // verify data if needed
if (verify && verifyBuffer) {
mpu6050_setMemoryBank(address, bank, 0, 0);
mpu6050_setMemoryStartAddress(address, memAddress);
i2c_read_bytes(address, MPU6050_MEM_R_W, chunkSize, verifyBuffer);
if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) {
free(verifyBuffer);
if (useProgMem) free(progBuffer);
return 0; // uh oh.
}
}*/
// increase byte index by [chunkSize]
i += chunkSize;
// unsigned char automatically wraps to 0 at 256
memAddress += chunkSize;
// if we aren't done, update bank (if necessary) and address
if (i < dataSize) {
if (memAddress == 0) bank++;
mpu6050_setMemoryBank(address, bank, 0, 0);
mpu6050_setMemoryStartAddress(address, memAddress);
}
}
//if (verify) free(verifyBuffer);
//if (useProgMem) free(progBuffer);
return 1;
}
