Chris Seto
/
MPU6050-DMP-4
MPU6050 issues
Fork of MPU6050 by
Diff: MPU6050.cpp
- Revision:
- 6:6300d9561dfd
- Parent:
- 3:25e1a5a10e53
diff -r 7d1bf3ce0053 -r 6300d9561dfd MPU6050.cpp --- a/MPU6050.cpp Sat Nov 23 16:47:00 2013 +0000 +++ b/MPU6050.cpp Tue Jul 01 16:56:00 2014 +0000 @@ -41,9 +41,11 @@ */ #include "MPU6050.h" +#include "shared.h" #define useDebugSerial + //instead of using pgmspace.h typedef const unsigned char prog_uchar; #define pgm_read_byte_near(x) (*(prog_uchar*)(x))//<- I modified here @@ -52,7 +54,7 @@ /** Default constructor, uses default I2C address. * @see MPU6050_DEFAULT_ADDRESS */ -MPU6050::MPU6050() : debugSerial(USBTX, USBRX) +MPU6050::MPU6050() { devAddr = MPU6050_DEFAULT_ADDRESS; } @@ -63,7 +65,7 @@ * @see MPU6050_ADDRESS_AD0_LOW * @see MPU6050_ADDRESS_AD0_HIGH */ -MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX) +MPU6050::MPU6050(uint8_t address) { devAddr = address; } @@ -79,7 +81,7 @@ { #ifdef useDebugSerial - debugSerial.printf("MPU6050::initialize start\n"); + pc.printf("MPU6050::initialize start\r\n"); #endif setClockSource(MPU6050_CLOCK_PLL_XGYRO); @@ -88,7 +90,7 @@ setSleepEnabled(false); // thanks to Jack Elston for pointing this one out! #ifdef useDebugSerial - debugSerial.printf("MPU6050::initialize end\n"); + pc.printf("MPU6050::initialize end\r\n"); #endif } @@ -99,11 +101,11 @@ bool MPU6050::testConnection() { #ifdef useDebugSerial - debugSerial.printf("MPU6050::testConnection start\n"); + pc.printf("MPU6050::testConnection start\r\n"); #endif uint8_t deviceId = getDeviceID(); #ifdef useDebugSerial - debugSerial.printf("DeviceId = %d\n",deviceId); + pc.printf("DeviceId = %d\r\n",deviceId); #endif return deviceId == 0x34; } @@ -3263,22 +3265,49 @@ 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;) { + + if (verify) + { + verifyBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + pc.printf("Verify\r\n"); + } + + if (useProgMem) + { + progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE); + pc.printf("Prog mem\r\n"); + } + + 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; + 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; + if (chunkSize > 256 - address) + { + chunkSize = 256 - address; + } + + pc.printf("Chunksize: %d\r\n", chunkSize); - if (useProgMem) { + if (useProgMem) + { // write the chunk of data as specified - for (j = 0; j < chunkSize; j++) progBuffer[j] = pgm_read_byte(data + i + j); - } else { + for (j = 0; j < chunkSize; j++) + { + progBuffer[j] = pgm_read_byte(data + i + j); + pc.printf("%d, ", progBuffer[j]); + } + } + else + { // write the chunk of data as specified progBuffer = (uint8_t *)data + i; } @@ -3286,11 +3315,14 @@ i2Cdev.writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer); // verify data if needed - if (verify && verifyBuffer) { + if (verify && verifyBuffer) + { setMemoryBank(bank); setMemoryStartAddress(address); i2Cdev.readBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, verifyBuffer); - if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) { + + if (memcmp(progBuffer, verifyBuffer, chunkSize) != 0) + { /*Serial.print("Block write verification error, bank "); Serial.print(bank, DEC); Serial.print(", address "); @@ -3340,7 +3372,9 @@ uint8_t success, special; uint8_t *progBuffer = NULL; uint16_t i, j; - if (useProgMem) { + + if (useProgMem) + { progBuffer = (uint8_t *)malloc(8); // assume 8-byte blocks, realloc later if necessary }