Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: leds.c
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
diff -r 1e3318a30ddd -r 90292f8bd179 leds.c --- 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;