Diff: Sensors/Gyro/L3G4200D.cpp

Revision:

20:e116e596e540

Parent:

17:e096e85f6c36

Child:

21:c2a2e7cbabdd

--- a/Sensors/Gyro/L3G4200D.cpp	Sat Nov 03 10:48:18 2012 +0000
+++ b/Sensors/Gyro/L3G4200D.cpp	Mon Nov 05 09:19:01 2012 +0000
@@ -35,9 +35,9 @@
     
     const int count = 50;
     for (int i = 0; i < count; i++) {                   // read 50 times the data in a very short time
-        read();
+        readraw();
         for (int j = 0; j < 3; j++)
-            Gyro_calib[j] += data[j];
+            Gyro_calib[j] += raw[j];
         wait(0.001);          // TODO: maybe less or no wait !!
     }
     
@@ -47,21 +47,24 @@
 
 void L3G4200D::read()
 {
-    char buffer[6];                                     // 8-Bit pieces of axis data
-    
-    //buffer[0] = L3G4200D_OUT_X_L | (1 << 7);          // TODO: wiiiiiiso?!
-    
-    readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C
+    readraw();                                          // read raw measurement data
     
-    data[0] = (short) (buffer[1] << 8 | buffer[0]);     // join 8-Bit pieces to 16-bit short integers
-    data[1] = (short) (buffer[3] << 8 | buffer[2]);
-    data[2] = (short) (buffer[5] << 8 | buffer[4]);
-    
-    for (int j = 0; j < 3; j++)
-            data[j] -= offset[j];                       // add offset from calibration
+    for (int i = 0; i < 3; i++)
+            data[i] = raw[i] - offset[i];               // subtract offset from calibration
 }
 
 int L3G4200D::readTemp()
 {
     return (short) readRegister(L3G4200D_OUT_TEMP);     // read the sensors register for the temperature
+}
+
+void L3G4200D::readraw()
+{
+    char buffer[6];                                     // 8-Bit pieces of axis data
+    
+    readMultiRegister(L3G4200D_OUT_X_L | (1 << 7), buffer, 6); // read axis registers using I2C   // TODO: wiiiiiiso?!   | (1 << 7)
+    
+    raw[0] = (short) (buffer[1] << 8 | buffer[0]);     // join 8-Bit pieces to 16-bit short integers
+    raw[1] = (short) (buffer[3] << 8 | buffer[2]);
+    raw[2] = (short) (buffer[5] << 8 | buffer[4]);
 }
\ No newline at end of file