Diff: MPU9150.cpp

Revision:

3:b0eb5b37ba29

Parent:

2:e523a92390b6

Child:

4:a4f794629c31

--- a/MPU9150.cpp	Mon Sep 01 14:25:06 2014 +0000
+++ b/MPU9150.cpp	Sun Mar 29 08:27:46 2015 +0000
@@ -790,12 +790,18 @@
    return write(reg_addr, &data, 1);
 }
 
-bool MPU9150::write(char reg_addr, char* data, int length){
-    i2c.start();
-    i2c.write(device_address << 1);
-    i2c.write(reg_addr);
-    for(int i = 0; i < length; i++) {
-        if(!i2c.write(data[i])){
+bool MPU9150::write(char reg_addr, char* data, int length)
+{
+    char reg_addrs[1] = {reg_addr};
+    char data_w[length];
+    for(int i=0; i<length; i++)
+        data_w[i] = data[i];
+    
+    i2c.write(device_address<<1, reg_addrs, 1, true);
+    for(int i = 0; i < length; i++) 
+    {
+        if(!i2c.write(data_w[i]))
+        {
             write_errors++;
             //debug.printf("Write Error %d\r\n", reg_addr);
             return false;
@@ -830,17 +836,32 @@
    return read(reg_addr, data, 1);
 }
 
-bool MPU9150::read(char reg_addr, char* data, int length){
-    if(i2c.write(device_address << 1, &reg_addr, 1, true)){
+bool MPU9150::read(char reg_addr, char* data, int length)
+{
+    char command[1];
+    command[0] = reg_addr;
+    char *redData = (char*)malloc(length);
+    
+    if(i2c.write(device_address<<1, command, 1, true))
+    {
         read_errors ++;
         //debug.printf("Read: Address Write Error %d\r\n", reg_addr);
         return false;
     }
-    if(i2c.read(device_address << 1, data, length)){
+    if(!i2c.read(device_address<<1, redData, length, false))
+    {
+         for(int i =0; i < length; i++) 
+        {
+            data[i] = redData[i];
+        }
+    }
+    else
+    {
         read_errors ++;
         //debug.printf("Read: Error %d\r\n", reg_addr);
         return false;
     }
+    free (redData);
     return true;
 }