Diff: leds.c

Revision:

2:90292f8bd179

Parent:

1:1e3318a30ddd

--- a/leds.c	Fri Feb 25 01:35:24 2011 +0000
+++ b/leds.c	Tue Apr 26 12:12:29 2011 +0000
@@ -2,7 +2,7 @@
 // =                              UAVXArm Quadrocopter Controller                                =
 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
-// =                     http://code.google.com/p/uavp-mods/ http://uavp.ch                      =
+// =                           http://code.google.com/p/uavp-mods/                               =
 // ===============================================================================================
 
 //    This is part of UAVXArm.
@@ -80,12 +80,19 @@
     }
 
     I2CLED.start();
-    I2CLED.write(PCA9551_ID);
-    I2CLED.write(0x15);
-    I2CLED.write(L03);
-    I2CLED.write(L47);
+    if ( I2CLED.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CLED.write(0x15) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CLED.write(L03) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CLED.write(L47) != I2C_ACK ) goto PCA9551Error;
     I2CLED.stop();
 
+    return;
+
+PCA9551Error:
+    I2CLED.stop();
+
+    I2CError[PCA9551_ID]++;
+
 } // WritePCA9551
 
 void SendLEDs(void) { // 39.3 uS @ 40MHz
@@ -204,16 +211,17 @@
 // LED Driver
 
 void PCA9551Test(void) {
-    static char b[8], i, r;
+    static char b[8];
+    static uint8 i;
 
     TxString("\r\nPCA9551Test\r\n");
 
     I2CLED.start();
-    I2CGYRO.write(PCA9551_ID);
-    I2CGYRO.write(0x11);
+    if ( I2CGYRO.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
+    if ( I2CGYRO.write(0x11) != I2C_ACK ) goto PCA9551Error;;
     I2CLED.stop();
 
-    if ( I2CLED.blockread(PCA9551_ID, b, 7) == 0 ) {
+    if ( I2CLED.blockread(PCA9551_ID, b, 7) ) goto PCA9551Error;
 
     TxString("0:\t0b");
     TxBin8(b[6]);
@@ -224,33 +232,47 @@
         TxBin8(b[i]);
         TxNextLine();
     }
-    }
-    else
+
+    TxNextLine();
+
+    return;
+
+PCA9551Error:
+    I2CLED.stop();
+
+    I2CError[PCA9551_ID]++;
+
     TxString("FAILED\r\n");
-    
-    TxNextLine();
+
 } // PCA9551Test
 
 boolean IsPCA9551Active(void) {
 
     const char b[7] = {0x11,0x25,0x80,0x25,0x80,0x00,0x00}; // Period 1Sec., PWM 50%, ON
-    boolean r;
 
     F.UsingLEDDriver = I2CGYROAddressResponds( PCA9551_ID );
 
     if ( F.UsingLEDDriver ) {
-        I2CLED.blockwrite(PCA9551_ID, b, 7);
+        if ( I2CLED.blockwrite(PCA9551_ID, b, 7) ) goto PCA9551Error;
         TrackMinI2CRate(400000);
     }
 
+#ifdef DISABLE_LED_DRIVER
+    F.UsingLEDDriver = false;
+#endif // DISABLE_LED_DRIVER
+
     return ( F.UsingLEDDriver );
-    
+
+PCA9551Error:
+
+    F.UsingLEDDriver = false;
+
+    return ( F.UsingLEDDriver );
+
 } //IsPCA9551Active
 
 void InitLEDs(void) {
 
-    boolean r;
-
     r = IsPCA9551Active();
 
     LEDShadow = SavedLEDs = LEDPattern = 0;