Diff: GY80/GY80.cpp

Revision:

2:9fe6f1e273b4

Parent:

0:5d27c333afa6

--- a/GY80/GY80.cpp	Tue Aug 26 06:46:55 2014 +0000
+++ b/GY80/GY80.cpp	Thu Aug 28 07:50:06 2014 +0000
@@ -2,6 +2,7 @@
 
 Serial pc2(USBTX, USBRX);
 
+
 GY80::GY80() : Wire( SDA,SCL)
 {
     Wire.frequency(I2C_FREQ);
@@ -73,16 +74,19 @@
     buff[0] = 0x32; // Send address to read from
     Wire.write(ACCEL_ADDRESS, buff, 1);
 
-    int accel[3];
+    //int accel[3];
     if (Wire.read(ACCEL_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        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] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        accel_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        accel_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
-    accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
+    /*accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
     accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
-    accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
+    accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;*/
+    /*accel_v[0] = accel[0]*0.5 + accel_v[0]*0.5;
+    accel_v[1] = accel[0]*0.5 + accel_v[0]*0.5;
+    accel_v[2] = accel[0]*0.5 + accel_v[0]*0.5;*/
 }
 
 
@@ -93,16 +97,16 @@
     buff[0] = 0xA8; // 0x28 | (1 << 7) Send address to read from 
     Wire.write(GYRO_ADDRESS, buff, 1);
     // Request 6 bytes
-    int gyro[3];
+    // int gyro[3];
     if (Wire.read(GYRO_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        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] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        gyro_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        gyro_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
-    gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); 
+    /*gyro_v[0] = DEG2RAD((gyro[0] - GYRO_X_OFFSET) * GYRO_GAIN_X); 
     gyro_v[1] = DEG2RAD((gyro[1] - GYRO_Y_OFFSET) * GYRO_GAIN_Y);
-    gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);
+    gyro_v[2] = DEG2RAD((gyro[2] - GYRO_Z_OFFSET) * GYRO_GAIN_Z);*/
 }
 
 void GY80::Read_Magn(float* magn_v)
@@ -113,15 +117,15 @@
     Wire.write(MAGN_ADDRESS, buff, 1);
 
     // Request 6 bytes
-    int mag[3];
+    //int mag[3];
     if (Wire.read(MAGN_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        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] = (short) ((uint16_t) buff[1] << 8 | buff[0]);
+        magn_v[1] = (short) ((uint16_t) buff[3] << 8 | buff[2]);
+        magn_v[2] = (short) ((uint16_t) buff[5] << 8 | buff[4]);
     }
-    magn_v[0] = (mag[0] - MAGN_X_OFFSET) * MAGN_X_SCALE;
+    /*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;
+    magn_v[2] = (mag[2] - MAGN_Z_OFFSET) * MAGN_Z_SCALE; */
 }