Diff: GY80.cpp

Revision:

1:6909d797972f

Parent:

0:336ec4d70363

Child:

2:2553ca95fba0

diff -r 336ec4d70363 -r 6909d797972f GY80.cpp
--- a/GY80.cpp	Fri Oct 04 05:27:10 2013 +0000
+++ b/GY80.cpp	Sat Oct 05 02:22:15 2013 +0000
@@ -1,12 +1,11 @@
 #include "GY80.h"
 
-
-GY80::GY80(PinName sda, PinName scl):Wire(sda,scl)
+GY80::GY80(PinName sda, PinName scl): Wire( sda, scl)
 {
+    Wire.frequency(I2C_FREQ);
     Accel_Init();
     Gyro_Init();
     Magn_Init();
-    Wire.frequency(I2C_FREQ);
 }
 
 void GY80::Accel_Init()
@@ -16,12 +15,12 @@
     data[1] = 0x08; //Measurement mode
     Wire.write(ACCEL_ADDRESS, data, 2);
     wait_ms(1);
-    
+
     data[0] = 0x31; // Data format register
     data[1] = 0x08; //Set to full resolution
     Wire.write(ACCEL_ADDRESS, data, 2);
     wait_ms(1);
-  
+
     // Because our main loop runs at 50Hz we adjust the output data rate to 50Hz (25Hz bandwidth)
     data[0] = 0x2C; // Rate
     data[1] = 0x0A; //Set to 50Hz, normal operation, 0x0A 100hz 
@@ -37,18 +36,18 @@
     data[1] = 0x0; // normal power mode, all axes enable, 100Hz
     Wire.write(GYRO_ADDRESS, data, 2);
     wait_ms(1);
-  
-    
+
+
     data[0] = 0x23; // L3G4200D_CTRL_REG4
     data[1] = 0x20; //2000 dps full scale 
     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
     Wire.write(GYRO_ADDRESS, data, 2);
@@ -62,7 +61,7 @@
     data[1] = 0x00; // Set continuous mode (default 10Hz)
     Wire.write(MAGN_ADDRESS, data, 2);
     wait_ms(1);
-    
+
     data[0] = 0x00;
     data[1] = 0x18;
     Wire.write(MAGN_ADDRESS, data, 2);
@@ -74,12 +73,12 @@
     byte buff[6];
     buff[0] = 0x32; // Send address to read from
     Wire.write(ACCEL_ADDRESS, buff, 1);
-       
+
     if (Wire.read(ACCEL_ADDRESS, buff,6) == 0)  // All bytes received?
     {
         Convert(accel_v, buff);
     }
-  // Why aren't we scaling accelerometer? I think the DCM paper talks a little about this... ??
+    // 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;
     accel_v[1] = (accel[1] - ACCEL_Y_OFFSET) * ACCEL_Y_SCALE;
     accel_v[2] = (accel[2] - ACCEL_Z_OFFSET) * ACCEL_Z_SCALE;
@@ -108,7 +107,7 @@
 
     buff[0] = 0x03; // Send address to read from
     Wire.write(MAGN_ADDRESS, buff, 1);
-    
+
     // Request 6 bytes
     if (Wire.read(MAGN_ADDRESS, buff,6) == 0)  // All bytes received?
     {
@@ -121,7 +120,7 @@
 
 void GY80::Convert(float* 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] = ((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)
 }
\ No newline at end of file