Diff: LSM303DLM.cpp

Revision:

1:fc5c9258ec45

Parent:

0:faef9e4c8bea

Child:

2:0fcea8569714

--- a/LSM303DLM.cpp	Fri Jan 20 23:47:25 2012 +0000
+++ b/LSM303DLM.cpp	Tue Jan 24 16:56:35 2012 +0000
@@ -17,20 +17,25 @@
     // continuous conversion mode
     _data[0] = MR_REG_M;
     _data[1] = 0x00;
+#ifdef DEBUG_I2C
     if (!_device.write(MAG_ADDRESS, _data, 2))
         fprintf(stdout, "MR_REG_M: no ack\n");
+#endif        
     // data rate 75hz
     _data[0] = CRA_REG_M;
     _data[1] = 0x18; // 0b00011000
+#ifdef DEBUG_I2C    
     if (_device.write(MAG_ADDRESS, _data, 2))
         fprintf(stdout, "CRA_REG_M: no ack\n");
-    
+#endif    
     // init acc
     // data rate 100hz 
     _data[0] = CTRL_REG1_A;
     _data[1] = 0x2F; // 0b00101111
+#ifdef DEBUG_I2C    
     if (!_device.write(ACC_ADDRESS, _data, 2))
         fprintf(stdout, "CTRL_REG1_A: no ack\n");
+#endif        
 }
 
 void LSM303DLM::read(int a[3], int m[3])
@@ -46,9 +51,9 @@
     _device.read(ACC_ADDRESS, _data, 6);
 
     // 12-bit values
-    a[0] = (short) (_data[0] | _data[1]<<8)>>4; 
-    a[1] = (short) (_data[2] | _data[3]<<8)>>4;
-    a[2] = (short) (_data[4] | _data[5]<<8)>>4;
+    a[0] = (short) (_data[1]<<8 | _data[0]) >> 4;
+    a[1] = (short) (_data[3]<<8 | _data[2]) >> 4;
+    a[2] = (short) (_data[5]<<8 | _data[4]) >> 4;
 }
 
 void LSM303DLM::readMag(int m[3])
@@ -57,7 +62,21 @@
     _device.write(MAG_ADDRESS, _data, 1);
     _device.read(MAG_ADDRESS, _data, 6);
     
-    m[0] = (short) _data[0]<<8 | _data[1]; // X
-    m[1] = (short) _data[4]<<8 | _data[5]; // Y
-    m[2] = (short) _data[2]<<8 | _data[3]; // Z
+    m[0] = (short) (_data[0]<<8 | _data[1]); // X
+    m[1] = (short) (_data[4]<<8 | _data[5]); // Y
+    m[2] = (short) (_data[2]<<8 | _data[3]); // Z
+ }
+
+void LSM303DLM::setScale(float x, float y, float z)
+{
+    scale[0] = x;
+    scale[1] = y;
+    scale[2] = z;
+}
+
+void LSM303DLM::setOffset(float x, float y, float z)
+{
+    offset[0] = x;
+    offset[1] = y;
+    offset[2] = z;
 }
\ No newline at end of file