Diff: MPU6050.cpp

Revision:

6:b272bd888e98

Parent:

3:25e1a5a10e53

diff -r 7d1bf3ce0053 -r b272bd888e98 MPU6050.cpp
--- a/MPU6050.cpp	Sat Nov 23 16:47:00 2013 +0000
+++ b/MPU6050.cpp	Wed Jul 02 19:38:47 2014 +0000
@@ -41,8 +41,9 @@
 */
 
 #include "MPU6050.h"
+#include "shared.h"
 
-#define useDebugSerial
+//#define useDebugSerial
 
 //instead of using pgmspace.h
 typedef const unsigned char prog_uchar;
@@ -52,7 +53,7 @@
 /** Default constructor, uses default I2C address.
  * @see MPU6050_DEFAULT_ADDRESS
  */
-MPU6050::MPU6050() : debugSerial(USBTX, USBRX)
+MPU6050::MPU6050() : i2Cdev(I2C_SDA, I2C_SCL)
 {
     devAddr = MPU6050_DEFAULT_ADDRESS;
 }
@@ -63,7 +64,7 @@
  * @see MPU6050_ADDRESS_AD0_LOW
  * @see MPU6050_ADDRESS_AD0_HIGH
  */
-MPU6050::MPU6050(uint8_t address) : debugSerial(USBTX, USBRX)
+MPU6050::MPU6050(uint8_t address) : i2Cdev(I2C_SDA, I2C_SCL)
 {
     devAddr = address;
 }
@@ -3263,27 +3264,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");
+    }
+        
+    if (useProgMem)
+    {
+        progBuffer = (uint8_t *)malloc(MPU6050_DMP_MEMORY_CHUNK_SIZE);
+        pc.printf("Prog mem");
+    }
+        
+    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;
+        
+        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;
         }
 
-        i2Cdev.writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer);
+        pc.printf("ERRO!: %d\r\n", i2Cdev.writeBytes(devAddr, MPU6050_RA_MEM_R_W, chunkSize, progBuffer));
 
         // verify data if needed
         if (verify && verifyBuffer) {