Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
leds.c
- Committer:
- gke
- Date:
- 2011-04-26
- Revision:
- 2:90292f8bd179
- Parent:
- 1:1e3318a30ddd
File content as of revision 2:90292f8bd179:
// =============================================================================================== // = 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/ = // =============================================================================================== // This is part of UAVXArm. // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU // General Public License as published by the Free Software Foundation, either version 3 of the // License, or (at your option) any later version. // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. // See the GNU General Public License for more details. // You should have received a copy of the GNU General Public License along with this program. // If not, see http://www.gnu.org/licenses/ #include "UAVXArm.h" void SendLEDs(void); void SaveLEDs(void); void RestoreLEDs(void); void LEDsOn(uint16); void LEDsOff(uint16); void LEDChaser(void); void PCA9551Test(void); void WritePCA9551(uint8); boolean IsPCA9551Active(void); void DoLEDs(void); void PowerOutput(int8); void LEDsAndBuzzer(void); void InitLEDs(void); uint16 LEDShadow, SavedLEDs, LEDPattern = 0; uint16 PrevLEDShadow; uint8 PrevPCA9551LEDShadow; boolean PrevHolding = false; const uint16 LEDChase[8] = { YellowM, RedM, GreenM, BlueM, DRV1M, DRV3M, DRV2M, DRV0M }; void WritePCA9551(uint8 S) { const uint8 M = 0; // On // const uint8 M = 2; // PWM0 rate // const uint8 M = 3; // PWM1 rate const uint8 LOFF = 1; // High impedance static uint8 L03, L47, i; L03 = L47 = 0; for ( i = 0; i < 4; i++ ) { L03 <<= 2; if ( S & 0x80 ) L03 |= M; else L03 |= LOFF; S <<= 1; } for ( i = 0; i <4; i++ ) { L47 <<= 2; if ( S & 0x80 ) L47 |= M; else L47 |= LOFF; S <<= 1; } I2CLED.start(); 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 static uint8 PCA9551LEDShadow; if ( LEDShadow != PrevLEDShadow ) { BlueLED = LEDShadow & BlueM; RedLED = LEDShadow & RedM; GreenLED = LEDShadow & GreenM; YellowLED = LEDShadow & YellowM; PCA9551LEDShadow = uint8(LEDShadow & 0x00ff); if ( F.UsingLEDDriver && ( PCA9551LEDShadow != PrevPCA9551LEDShadow ) ) { WritePCA9551(PCA9551LEDShadow); PrevPCA9551LEDShadow = PCA9551LEDShadow; } PrevLEDShadow = LEDShadow; } } // SendLEDs void SaveLEDs(void) { // one level only SavedLEDs = LEDShadow; } // SaveLEDs void RestoreLEDs(void) { LEDShadow = SavedLEDs; SendLEDs(); } // RestoreLEDs void LEDsOn(uint16 l) { LEDShadow |= l; SendLEDs(); } // LEDsOn void LEDsOff(uint16 l) { LEDShadow &= ~l; SendLEDs(); } // LEDsOff void LEDChaser(void) { #define LED_NO (uint8)6 // skip buzzer //#define LED_NO (uint8)7 // all LEDs if ( mSClock() > mS[LEDChaserUpdate] ) { if ( F.HoldingAlt ) { LEDShadow ^= LEDChase[LEDPattern]; if ( LEDPattern < LED_NO ) LEDPattern++; else LEDPattern = 0; LEDShadow |= LEDChase[LEDPattern]; SendLEDs(); } else { LEDShadow = SavedLEDs; SendLEDs(); } mS[LEDChaserUpdate] = mSClock() + 50; } } // LEDChaser void DoLEDs(void) { if ( F.AccelerationsValid ) LEDYellow_ON; else LEDYellow_OFF; if ( F.Signal ) { LEDRed_OFF; LEDGreen_ON; } else { LEDGreen_OFF; LEDRed_ON; } } // DoLEDs void PowerOutput(int8 d) { int8 s; uint16 m; m = 1 << d; for ( s=0; s < 10; s++ ) { // 10 flashes (count MUST be even!) LEDShadow ^= m; SendLEDs(); Delay1mS(50); } } // PowerOutput void LEDsAndBuzzer(void) { int8 s, m; uint16 mask, LEDSave; LEDSave = LEDShadow; LEDShadow = 0; SendLEDs(); TxString("\r\nOutput test\r\n"); TxString("Sequence Drv0/Buzz, Drv1, Drv2, Drv3, Aux0, Aux1, Aux2, Aux3, Y, R, G, B\r\n"); mask = (uint8)1; for ( m = 1; m <= 12; m++ ) { for ( s = 0; s < 10; s++ ) { // 10 flashes (count MUST be even!) LEDShadow ^= mask; SendLEDs(); Delay1mS(100); } mask <<= 1; } LEDShadow = LEDSave; SendLEDs(); TxString("Test Finished\r\n"); } // LEDsAndBuzzer //______________________________________________________________________________________________ // LED Driver void PCA9551Test(void) { static char b[8]; static uint8 i; TxString("\r\nPCA9551Test\r\n"); I2CLED.start(); 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) ) goto PCA9551Error; TxString("0:\t0b"); TxBin8(b[6]); TxNextLine(); for (i = 0; i <6; i++ ) { TxVal32(i+1, 0,0); TxString(":\t0b"); TxBin8(b[i]); TxNextLine(); } TxNextLine(); return; PCA9551Error: I2CLED.stop(); I2CError[PCA9551_ID]++; TxString("FAILED\r\n"); } // PCA9551Test boolean IsPCA9551Active(void) { const char b[7] = {0x11,0x25,0x80,0x25,0x80,0x00,0x00}; // Period 1Sec., PWM 50%, ON F.UsingLEDDriver = I2CGYROAddressResponds( PCA9551_ID ); if ( F.UsingLEDDriver ) { 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) { r = IsPCA9551Active(); LEDShadow = SavedLEDs = LEDPattern = 0; PrevLEDShadow = 0x0fff; PrevPCA9551LEDShadow = 0; WritePCA9551( PrevPCA9551LEDShadow ); } // InitLEDs