Diff: gyro.c

Revision:

1:1e3318a30ddd

Parent:

0:62a1c91a859a

Child:

2:90292f8bd179

--- a/gyro.c	Fri Feb 18 22:28:05 2011 +0000
+++ b/gyro.c	Fri Feb 25 01:35:24 2011 +0000
@@ -210,55 +210,53 @@
 
 void ReadITG3200Gyro(void) {
     static char G[6];
-    static uint8 g;
+    static uint8 g, r;
     static i16u GX, GY, GZ;
 
     I2CGYRO.start();
-    if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
-    if ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK ) goto SGerror;
+    r = ( I2CGYRO.write(ITG3200_WR) != I2C_ACK );
+    r = ( I2CGYRO.write(ITG3200_GX_H) != I2C_ACK );
     I2CGYRO.stop();
 
-    I2CGYRO.read(ITG3200_R, G, 6);
+    if ( I2CGYRO.blockread(ITG3200_ID, G, 6) == 0 ) {
 
-    GX.b0 = G[1];
-    GX.b1 = G[0];
-    GY.b0 = G[3];
-    GY.b1 = G[2];
-    GZ.b0 = G[5];
-    GZ.b1 = G[4];
+        GX.b0 = G[1];
+        GX.b1 = G[0];
+        GY.b0 = G[3];
+        GY.b1 = G[2];
+        GZ.b0 = G[5];
+        GZ.b1 = G[4];
 
-    if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up 
-        GyroADC[Roll] = -(real32)GY.i16;
-        GyroADC[Pitch] = -(real32)GX.i16;
-        GyroADC[Yaw] = -(real32)GZ.i16;
-    } else { // SparkFun 6DOF breakout pins forward components down
-        GyroADC[Roll] = -(real32)GX.i16;
-        GyroADC[Pitch] = -(real32)GY.i16;
-        GyroADC[Yaw] = (real32)GZ.i16;
+        if ( F.Using9DOF ) { // SparkFun/QuadroUFO breakout pins forward components up
+            GyroADC[Roll] = -(real32)GY.i16;
+            GyroADC[Pitch] = -(real32)GX.i16;
+            GyroADC[Yaw] = -(real32)GZ.i16;
+        } else { // SparkFun 6DOF breakout pins forward components down
+            GyroADC[Roll] = -(real32)GX.i16;
+            GyroADC[Pitch] = -(real32)GY.i16;
+            GyroADC[Yaw] = (real32)GZ.i16;
+        }
+
+        for ( g = 0; g < (uint8)3; g++ )
+            GyroADC[g] *= GyroToRadian[ITG3200Gyro];
+
+    } else {
+        // GYRO FAILURE - FATAL
+        Stats[GyroFailS]++;
+      // not in flight keep trying   F.GyroFailure = true;
     }
 
-    for ( g = 0; g < (uint8)3; g++ )
-        GyroADC[g] *= GyroToRadian[ITG3200Gyro];
-
-    return;
-
-SGerror:
-    I2CGYRO.stop();
-    // GYRO FAILURE - FATAL
-    Stats[GyroFailS]++;
-    F.GyroFailure = true;
-    return;
 } // ReadITG3200Gyro
 
 uint8 ReadByteITG3200(uint8 a) {
     static uint8 d;
 
     I2CGYRO.start();
-    if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror;
     if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
 
     I2CGYRO.start();
-    if ( I2CGYRO.write(ITG3200_R) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(ITG3200_RD) != I2C_ACK ) goto SGerror;
     d = I2CGYRO.read(I2C_NACK);
     I2CGYRO.stop();
 
@@ -274,7 +272,7 @@
 
 void WriteByteITG3200(uint8 a, uint8 d) {
     I2CGYRO.start();    // restart
-    if ( I2CGYRO.write(ITG3200_W) != I2C_ACK ) goto SGerror;
+    if ( I2CGYRO.write(ITG3200_WR) != I2C_ACK ) goto SGerror;
     if ( I2CGYRO.write(a) != I2C_ACK ) goto SGerror;
     if ( I2CGYRO.write(d) != I2C_ACK ) goto SGerror;
     I2CGYRO.stop();
@@ -352,10 +350,9 @@
 } // ITG3200Test
 
 boolean ITG3200GyroActive(void) {
-    I2CGYRO.start();
-    F.GyroFailure = I2CGYRO.write(ITG3200_ID) != I2C_ACK;
-    I2CGYRO.stop();
-    
+
+    F.GyroFailure = !I2CGYROAddressResponds( ITG3200_ID );
+
     if ( !F.GyroFailure )
         TrackMinI2CRate(400000);