leds.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 void SendLEDs(void);
00024 void SaveLEDs(void);
00025 void RestoreLEDs(void);
00026 void LEDsOn(uint16);
00027 void LEDsOff(uint16);
00028 void LEDChaser(void);
00029 
00030 void PCA9551Test(void);
00031 void WritePCA9551(uint8);
00032 boolean IsPCA9551Active(void);
00033 
00034 void DoLEDs(void);
00035 void PowerOutput(int8);
00036 void LEDsAndBuzzer(void);
00037 void InitLEDs(void);
00038 
00039 uint16 LEDShadow, SavedLEDs, LEDPattern = 0;
00040 uint16 PrevLEDShadow;
00041 uint8 PrevPCA9551LEDShadow;
00042 
00043 boolean PrevHolding = false;
00044 const uint16 LEDChase[8] = {
00045     YellowM,
00046     RedM,
00047     GreenM,
00048     BlueM,
00049     DRV1M,
00050     DRV3M,
00051     DRV2M,
00052     DRV0M
00053 };
00054 
00055 void WritePCA9551(uint8 S) {
00056     const uint8 M = 0; // On
00057     //  const uint8 M = 2; // PWM0 rate
00058     //  const uint8 M = 3; // PWM1 rate
00059 
00060     const uint8 LOFF = 1; // High impedance
00061     static uint8 L03, L47, i;
00062 
00063     L03 = L47 = 0;
00064     for ( i = 0; i < 4; i++ ) {
00065         L03 <<= 2;
00066         if ( S & 0x80 )
00067             L03 |= M;
00068         else
00069             L03 |= LOFF;
00070         S  <<= 1;
00071     }
00072 
00073     for ( i = 0; i <4; i++ ) {
00074         L47 <<= 2;
00075         if ( S & 0x80 )
00076             L47 |= M;
00077         else
00078             L47 |= LOFF;
00079         S  <<= 1;
00080     }
00081 
00082     I2CLED.start();
00083     if ( I2CLED.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
00084     if ( I2CLED.write(0x15) != I2C_ACK ) goto PCA9551Error;
00085     if ( I2CLED.write(L03) != I2C_ACK ) goto PCA9551Error;
00086     if ( I2CLED.write(L47) != I2C_ACK ) goto PCA9551Error;
00087     I2CLED.stop();
00088 
00089     return;
00090 
00091 PCA9551Error:
00092     I2CLED.stop();
00093 
00094     I2CError[PCA9551_ID]++;
00095 
00096 } // WritePCA9551
00097 
00098 void SendLEDs(void) { // 39.3 uS @ 40MHz
00099     static uint8 PCA9551LEDShadow;
00100 
00101     if ( LEDShadow != PrevLEDShadow ) {
00102 
00103         BlueLED = LEDShadow & BlueM;
00104         RedLED = LEDShadow & RedM;
00105         GreenLED = LEDShadow & GreenM;
00106         YellowLED = LEDShadow & YellowM;
00107 
00108         PCA9551LEDShadow = uint8(LEDShadow & 0x00ff);
00109 
00110         if ( F.UsingLEDDriver && ( PCA9551LEDShadow != PrevPCA9551LEDShadow ) ) {
00111             WritePCA9551(PCA9551LEDShadow);
00112             PrevPCA9551LEDShadow = PCA9551LEDShadow;
00113         }
00114 
00115         PrevLEDShadow = LEDShadow;
00116     }
00117 
00118 } // SendLEDs
00119 
00120 
00121 void SaveLEDs(void) { // one level only
00122     SavedLEDs = LEDShadow;
00123 } // SaveLEDs
00124 
00125 void RestoreLEDs(void) {
00126     LEDShadow = SavedLEDs;
00127     SendLEDs();
00128 } // RestoreLEDs
00129 
00130 void LEDsOn(uint16 l) {
00131     LEDShadow |= l;
00132     SendLEDs();
00133 } // LEDsOn
00134 
00135 void LEDsOff(uint16 l) {
00136     LEDShadow &= ~l;
00137     SendLEDs();
00138 } // LEDsOff
00139 
00140 void LEDChaser(void) {
00141 #define LED_NO  (uint8)6    // skip buzzer
00142 //#define LED_NO  (uint8)7    // all LEDs
00143 
00144     if ( mSClock() > mS[LEDChaserUpdate] ) {
00145         if ( F.HoldingAlt ) {
00146             LEDShadow ^= LEDChase[LEDPattern];
00147             if ( LEDPattern < LED_NO ) LEDPattern++;
00148             else LEDPattern = 0;
00149             LEDShadow |= LEDChase[LEDPattern];
00150             SendLEDs();
00151         } else {
00152             LEDShadow = SavedLEDs;
00153             SendLEDs();
00154         }
00155 
00156         mS[LEDChaserUpdate] = mSClock() + 50;
00157     }
00158 } // LEDChaser
00159 
00160 void DoLEDs(void) {
00161     if ( F.AccelerationsValid  ) LEDYellow_ON;
00162     else LEDYellow_OFF;
00163 
00164     if ( F.Signal ) {
00165         LEDRed_OFF;
00166         LEDGreen_ON;
00167     } else {
00168         LEDGreen_OFF;
00169         LEDRed_ON;
00170     }
00171 } // DoLEDs
00172 
00173 void PowerOutput(int8 d) {
00174     int8 s;
00175     uint16 m;
00176 
00177     m = 1 << d;
00178     for ( s=0; s < 10; s++ ) { // 10 flashes (count MUST be even!)
00179         LEDShadow ^= m;
00180         SendLEDs();
00181         Delay1mS(50);
00182     }
00183 } // PowerOutput
00184 
00185 void LEDsAndBuzzer(void) {
00186     int8 s, m;
00187     uint16 mask, LEDSave;
00188 
00189     LEDSave = LEDShadow;
00190     LEDShadow  = 0;
00191     SendLEDs();
00192 
00193     TxString("\r\nOutput test\r\n");
00194     TxString("Sequence Drv0/Buzz, Drv1, Drv2, Drv3, Aux0, Aux1, Aux2, Aux3, Y, R, G, B\r\n");
00195     mask = (uint8)1;
00196     for ( m = 1; m <= 12; m++ ) {
00197         for ( s = 0; s < 10; s++ ) { // 10 flashes (count MUST be even!)
00198             LEDShadow ^= mask;
00199             SendLEDs();
00200             Delay1mS(100);
00201         }
00202         mask <<= 1;
00203     }
00204     LEDShadow  = LEDSave;
00205     SendLEDs();
00206     TxString("Test Finished\r\n");
00207 } // LEDsAndBuzzer
00208 
00209 //______________________________________________________________________________________________
00210 
00211 // LED Driver
00212 
00213 void PCA9551Test(void) {
00214     static char b[8];
00215     static uint8 i;
00216 
00217     TxString("\r\nPCA9551Test\r\n");
00218 
00219     I2CLED.start();
00220     if ( I2CGYRO.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
00221     if ( I2CGYRO.write(0x11) != I2C_ACK ) goto PCA9551Error;;
00222     I2CLED.stop();
00223 
00224     if ( I2CLED.blockread(PCA9551_ID, b, 7) ) goto PCA9551Error;
00225 
00226     TxString("0:\t0b");
00227     TxBin8(b[6]);
00228     TxNextLine();
00229     for (i = 0; i <6; i++ ) {
00230         TxVal32(i+1, 0,0);
00231         TxString(":\t0b");
00232         TxBin8(b[i]);
00233         TxNextLine();
00234     }
00235 
00236     TxNextLine();
00237 
00238     return;
00239 
00240 PCA9551Error:
00241     I2CLED.stop();
00242 
00243     I2CError[PCA9551_ID]++;
00244 
00245     TxString("FAILED\r\n");
00246 
00247 } // PCA9551Test
00248 
00249 boolean IsPCA9551Active(void) {
00250 
00251     const char b[7] = {0x11,0x25,0x80,0x25,0x80,0x00,0x00}; // Period 1Sec., PWM 50%, ON
00252 
00253     F.UsingLEDDriver = I2CGYROAddressResponds( PCA9551_ID );
00254 
00255     if ( F.UsingLEDDriver ) {
00256         if ( I2CLED.blockwrite(PCA9551_ID, b, 7) ) goto PCA9551Error;
00257         TrackMinI2CRate(400000);
00258     }
00259 
00260 #ifdef DISABLE_LED_DRIVER
00261     F.UsingLEDDriver = false;
00262 #endif // DISABLE_LED_DRIVER
00263 
00264     return ( F.UsingLEDDriver );
00265 
00266 PCA9551Error:
00267 
00268     F.UsingLEDDriver = false;
00269 
00270     return ( F.UsingLEDDriver );
00271 
00272 } //IsPCA9551Active
00273 
00274 void InitLEDs(void) {
00275 
00276     r = IsPCA9551Active();
00277 
00278     LEDShadow = SavedLEDs = LEDPattern = 0;
00279     PrevLEDShadow = 0x0fff;
00280 
00281     PrevPCA9551LEDShadow = 0;
00282     WritePCA9551( PrevPCA9551LEDShadow );
00283 
00284 } // InitLEDs
00285