Diff: GY80.cpp

Revision:

2:2553ca95fba0

Parent:

1:6909d797972f

Child:

3:981c93d62e16

--- a/GY80.cpp	Sat Oct 05 02:22:15 2013 +0000
+++ b/GY80.cpp	Sun Oct 06 07:09:50 2013 +0000
@@ -1,13 +1,17 @@
 #include "GY80.h"
 
-GY80::GY80(PinName sda, PinName scl): Wire( sda, scl)
+Serial pc2(USBTX, USBRX);
+
+GY80::GY80() : Wire( SDA,SCL)
 {
     Wire.frequency(I2C_FREQ);
     Accel_Init();
     Gyro_Init();
     Magn_Init();
 }
-
+GY80::~GY80()
+{
+}
 void GY80::Accel_Init()
 {    
     byte data[2];
@@ -33,7 +37,7 @@
     byte data[2];
 
     data[0] = 0x20; //L3G4200D_CTRL_REG1
-    data[1] = 0x0; // normal power mode, all axes enable, 100Hz
+    data[1] = 0x0F; // normal power mode, all axes enable, 100Hz
     Wire.write(GYRO_ADDRESS, data, 2);
     wait_ms(1);
 
@@ -51,7 +55,6 @@
     data[0] = 0x24; // L3G4200D_CTRL_REG5
     data[1] = 0x02; //Low Pass Filter
     Wire.write(GYRO_ADDRESS, data, 2);
-    wait_ms(1);
 }
 
 void GY80::Magn_Init()
@@ -76,7 +79,7 @@
 
     if (Wire.read(ACCEL_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        Convert(accel_v, buff);
+        Convert(accel, buff);
     }
     // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
     accel_v[0] = (accel[0] - ACCEL_X_OFFSET) * ACCEL_X_SCALE;
@@ -94,7 +97,7 @@
     // Request 6 bytes
     if (Wire.read(GYRO_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        Convert(gyro_v, buff);
+        Convert(gyro, buff);
     }
     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,16 +114,16 @@
     // Request 6 bytes
     if (Wire.read(MAGN_ADDRESS, buff,6) == 0)  // All bytes received?
     {
-        Convert(magn_v, buff);
+        Convert(mag, buff);
     }
     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(float* value, byte* raw)
+void GY80::Convert(short* value, byte* raw)
 {
-    value[0] = ((signed short) raw[1] << 8) | raw[0];  // Y axis (internal sensor x axis)
-    value[1] = ((signed short) raw[3] << 8) | raw[2];  // X axis (internal sensor y axis)
-    value[2] = ((signed short) raw[5] << 8) | raw[4];  // Z axis (internal sensor z axis)
+    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