Diff: GY80.cpp

Revision:

4:cc026f60357a

Parent:

3:981c93d62e16

Child:

5:9dc0f2d15783

--- a/GY80.cpp	Tue Oct 15 16:31:41 2013 +0000
+++ b/GY80.cpp	Sat Oct 19 17:33:40 2013 +0000
@@ -27,7 +27,7 @@
 
     // 
     data[0] = 0x2C; // Rate
-    data[1] = 0x0AC; //Set to 400Hz, normal operation, 0x0A 100hz 
+    data[1] = 0x0D; //Set to 800Hz, normal operation, 0x0A 100hz 
     Wire.write(ACCEL_ADDRESS, data, 2);
     wait_ms(1);
 }
@@ -37,7 +37,7 @@
     byte data[2];
 
     data[0] = 0x20; //L3G4200D_CTRL_REG1
-    data[1] = 0x8F; // normal power mode, all axes enable, 8:20 9:25 A:50 B:110 
+    data[1] = 0xCF; // normal power mode, all axes enable, 8:20 9:25 A:50 B:110 
     Wire.write(GYRO_ADDRESS, data, 2);
     wait_ms(1);
 
@@ -47,10 +47,6 @@
     Wire.write(GYRO_ADDRESS, data, 2);
     wait_ms(1);
 
-    data[0] = 0x21; // L3G4200D_CTRL_REG2
-    data[1] = 0x20; //High pass cutoff freq
-    Wire.write(GYRO_ADDRESS, data, 2);
-    wait_ms(1);  
 
     data[0] = 0x24; // L3G4200D_CTRL_REG5
     data[1] = 0x02; //Low Pass Filter
@@ -77,9 +73,12 @@
     buff[0] = 0x32; // Send address to read from
     Wire.write(ACCEL_ADDRESS, buff, 1);
 
+    int accel[3];
     if (Wire.read(ACCEL_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        Convert(accel, buff);
+        accel[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        accel[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        accel[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
     accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
     accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
@@ -94,9 +93,12 @@
     buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from 
     Wire.write(GYRO_ADDRESS, buff, 1);
     // Request 6 bytes
+    int gyro[3];
     if (Wire.read(GYRO_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        Convert(gyro, buff);
+        gyro[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        gyro[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        gyro[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
     gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); 
     gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
@@ -111,18 +113,15 @@
     Wire.write(MAGN_ADDRESS, buff, 1);
 
     // Request 6 bytes
+    int mag[3];
     if (Wire.read(MAGN_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        Convert(mag, buff);
+        mag[0] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        mag[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        mag[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
     magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
     magn_v[1] = (mag[1] - MAGN_Y_OFFSET) * MAGN_Y_SCALE;
     magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE;
 }
 
-void GY80::Convert(short* value, byte* raw)
-{
-    value[0] = (short) ((uint16_t) raw[1] << 8 | raw[0]);
-    value[1] = (short) ((uint16_t) raw[3] << 8 | raw[2]);
-    value[2] = (short) ((uint16_t) raw[5] << 8 | raw[4]);
-}
\ No newline at end of file