Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
Diff: leds.c
- Revision:
- 0:62a1c91a859a
- Child:
- 1:1e3318a30ddd
diff -r 000000000000 -r 62a1c91a859a leds.c --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/leds.c Fri Feb 18 22:28:05 2011 +0000 @@ -0,0 +1,265 @@ +// =============================================================================================== +// = 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 = +// =============================================================================================== + +// 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(); + I2CLED.write(PCA9551_ID); + I2CLED.write(0x15); + I2CLED.write(L03); + I2CLED.write(L47); + I2CLED.stop(); + +} // 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], i, r; + + TxString("\r\nPCA9551Test\r\n"); + + I2CLED.start(); + I2CGYRO.write(PCA9551_ID); + I2CGYRO.write(0x11); + I2CLED.stop(); + + I2CLED.read(PCA9551_ID, b, 7); + + 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(); +} // PCA9551Test + +boolean IsPCA9551Active(void) { + + const char b[7] = {0x11,0x25,0x80,0x25,0x80,0x00,0x00}; // Period 1Sec., PWM 50%, ON + + F.UsingLEDDriver = true; // optimistic + + I2CLED.start(); + if ( I2CGYRO.write(PCA9551_ID) != I2C_ACK ) goto LError; + I2CLED.stop(); + + I2CLED.write(PCA9551_ID, b, 7); + + TrackMinI2CRate(400000); + + return ( true ); + +LError: + F.UsingLEDDriver = false; + return ( F.UsingLEDDriver ); + +} //IsPCA9551Active + +void InitLEDs(void) { + + boolean r; + + r = IsPCA9551Active(); + + LEDShadow = SavedLEDs = LEDPattern = 0; + PrevLEDShadow = 0x0fff; + + PrevPCA9551LEDShadow = 0; + WritePCA9551( PrevPCA9551LEDShadow ); + +} // InitLEDs +