Diff: 9DOF.h

Revision:

24:e8fd339db02e

Parent:

23:0baaa24e04d8

--- a/9DOF.h	Sun Jan 11 19:54:10 2015 +0000
+++ b/9DOF.h	Mon Jan 12 13:47:19 2015 +0000
@@ -32,6 +32,7 @@
 void read_itg3200(I2C i2c, int *data);
 void init_hmc5883(I2C i2c);
 void read_hmc5883(I2C i2c, int *data);
+void twosComp2int(int *data, int size);
 
 //This function initializes the Digital Accelerometer ADXL345
 void init_adxl345(I2C i2c) {
@@ -56,6 +57,7 @@
     for (int i=0;i<3;++i) {
         data[i] = (int)bytes[2*i] + (((int)bytes[2*i + 1]) << 8);
     }
+    twosComp2int(data, 3);
 }
 
 
@@ -81,6 +83,7 @@
     for (int i=0;i<3;++i) {
         data[i] = (int)bytes[2*i + 1] + (((int)bytes[2*i]) << 8);
     }
+    twosComp2int(data, 3);
 }
 
 void init_hmc5883(I2C i2c) {
@@ -89,7 +92,9 @@
     data[1] = HMC5883_MEASMODE_CONT;
   
     i2c.write(HMC5883_ADDRESS_W, data, 2);
-    i2c.read(HMC5883_ADDRESS_R, data, 1);
+    
+    i2c.write(HMC5883_ADDRESS_W, data, 1);
+    i2c.read(HMC5883_ADDRESS_R, data, 2);
 }
 
 void read_hmc5883(I2C i2c, int *data) {
@@ -103,6 +108,14 @@
     for (int i=0;i<3;++i) {
         data[i] = (int)bytes[2*i + 1] + (((int)bytes[2*i]) << 8);
     }
+    twosComp2int(data, 3);
+}
+
+void twosComp2int(int *data, int size) {
+    for(int i = 0; i < size; i++){
+        if(data[i] >= 32768)
+            data[i] = -(data[i] - 32768 + 1);
+    }
 }
 
 #endif
\ No newline at end of file