Diff: LSM303DLHC.cpp

Revision:

6:fcf7e9b8ce21

Parent:

5:5f3ae033c62d

Child:

7:ddd9717cdb71

--- a/LSM303DLHC.cpp	Tue Nov 12 17:25:40 2013 +0000
+++ b/LSM303DLHC.cpp	Tue Nov 12 18:30:37 2013 +0000
@@ -109,6 +109,7 @@
 
 bool LSM303DLHC::read(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
     char acc[6], mag[6];
+    char temp;
  
     if (recv(addr_acc, OUT_X_A, acc, 6) && recv(addr_mag, OUT_X_M, mag, 6)) {
         *ax = *((short*)(acc))/8192.0;  //32768/4=8192
@@ -116,9 +117,15 @@
         *az = *((short*)(acc+4))/8192.0;
         //full scale magnetic readings are from -2048 to 2047
         //gain is x,y =1100; z = 980 LSB/gauss
-        *mx = ((((short)mag[0]) << 8) | (0xff&mag[1]))/1100.0;
-        *my = ((((short)mag[2]) << 8) | (0xff&mag[3]))/1100.0;
-        *mz = ((((short)mag[4]) << 8) | (0xff&mag[5]))/980.0;
+        for(int i=0; i<6; i+=2)
+        {
+            temp = mag[i];
+            mag[i] = mag[i+1];
+            mag[i+1] = temp;
+        }
+        *mx = *((short*)(mag))/1100.0;
+        *my = *((short*)(mag+2))/1100.0;
+        *mz = *((short*)(mag+4))/980.0;
  
         return true;
     }