Diff: MPU6050.cpp

Revision:

6:6300d9561dfd

Parent:

3:25e1a5a10e53

--- 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
     }