Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

leds.c@2:90292f8bd179, 2011-04-26 (annotated)

Committer:
gke
Date:
Tue Apr 26 12:12:29 2011 +0000
Revision:
2:90292f8bd179
Parent:
1:1e3318a30ddd 
Not flightworthy. Posted for others to make use of the I2C SW code.

Who changed what in which revision?

UserRevisionLine numberNew contents of line
gke 0:62a1c91a859a 1 // ===============================================================================================
gke 0:62a1c91a859a 2 // = UAVXArm Quadrocopter Controller =
gke 0:62a1c91a859a 3 // = Copyright (c) 2008 by Prof. Greg Egan =
gke 0:62a1c91a859a 4 // = Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer =
gke 2:90292f8bd179 5 // = http://code.google.com/p/uavp-mods/ =
gke 0:62a1c91a859a 6 // ===============================================================================================
gke 0:62a1c91a859a 7
gke 0:62a1c91a859a 8 // This is part of UAVXArm.
gke 0:62a1c91a859a 9
gke 0:62a1c91a859a 10 // UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
gke 0:62a1c91a859a 11 // General Public License as published by the Free Software Foundation, either version 3 of the
gke 0:62a1c91a859a 12 // License, or (at your option) any later version.
gke 0:62a1c91a859a 13
gke 0:62a1c91a859a 14 // UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
gke 0:62a1c91a859a 15 // even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
gke 0:62a1c91a859a 16 // See the GNU General Public License for more details.
gke 0:62a1c91a859a 17
gke 0:62a1c91a859a 18 // You should have received a copy of the GNU General Public License along with this program.
gke 0:62a1c91a859a 19 // If not, see http://www.gnu.org/licenses/
gke 0:62a1c91a859a 20
gke 0:62a1c91a859a 21 #include "UAVXArm.h"
gke 0:62a1c91a859a 22
gke 0:62a1c91a859a 23 void SendLEDs(void);
gke 0:62a1c91a859a 24 void SaveLEDs(void);
gke 0:62a1c91a859a 25 void RestoreLEDs(void);
gke 0:62a1c91a859a 26 void LEDsOn(uint16);
gke 0:62a1c91a859a 27 void LEDsOff(uint16);
gke 0:62a1c91a859a 28 void LEDChaser(void);
gke 0:62a1c91a859a 29
gke 0:62a1c91a859a 30 void PCA9551Test(void);
gke 0:62a1c91a859a 31 void WritePCA9551(uint8);
gke 0:62a1c91a859a 32 boolean IsPCA9551Active(void);
gke 0:62a1c91a859a 33
gke 0:62a1c91a859a 34 void DoLEDs(void);
gke 0:62a1c91a859a 35 void PowerOutput(int8);
gke 0:62a1c91a859a 36 void LEDsAndBuzzer(void);
gke 0:62a1c91a859a 37 void InitLEDs(void);
gke 0:62a1c91a859a 38
gke 0:62a1c91a859a 39 uint16 LEDShadow, SavedLEDs, LEDPattern = 0;
gke 0:62a1c91a859a 40 uint16 PrevLEDShadow;
gke 0:62a1c91a859a 41 uint8 PrevPCA9551LEDShadow;
gke 0:62a1c91a859a 42
gke 0:62a1c91a859a 43 boolean PrevHolding = false;
gke 0:62a1c91a859a 44 const uint16 LEDChase[8] = {
gke 0:62a1c91a859a 45 YellowM,
gke 0:62a1c91a859a 46 RedM,
gke 0:62a1c91a859a 47 GreenM,
gke 1:1e3318a30ddd 48 BlueM,
gke 0:62a1c91a859a 49 DRV1M,
gke 0:62a1c91a859a 50 DRV3M,
gke 0:62a1c91a859a 51 DRV2M,
gke 0:62a1c91a859a 52 DRV0M
gke 0:62a1c91a859a 53 };
gke 0:62a1c91a859a 54
gke 0:62a1c91a859a 55 void WritePCA9551(uint8 S) {
gke 0:62a1c91a859a 56 const uint8 M = 0; // On
gke 0:62a1c91a859a 57 // const uint8 M = 2; // PWM0 rate
gke 0:62a1c91a859a 58 // const uint8 M = 3; // PWM1 rate
gke 0:62a1c91a859a 59
gke 0:62a1c91a859a 60 const uint8 LOFF = 1; // High impedance
gke 0:62a1c91a859a 61 static uint8 L03, L47, i;
gke 0:62a1c91a859a 62
gke 0:62a1c91a859a 63 L03 = L47 = 0;
gke 0:62a1c91a859a 64 for ( i = 0; i < 4; i++ ) {
gke 0:62a1c91a859a 65 L03 <<= 2;
gke 0:62a1c91a859a 66 if ( S & 0x80 )
gke 0:62a1c91a859a 67 L03 |= M;
gke 0:62a1c91a859a 68 else
gke 0:62a1c91a859a 69 L03 |= LOFF;
gke 0:62a1c91a859a 70 S <<= 1;
gke 0:62a1c91a859a 71 }
gke 0:62a1c91a859a 72
gke 0:62a1c91a859a 73 for ( i = 0; i <4; i++ ) {
gke 0:62a1c91a859a 74 L47 <<= 2;
gke 0:62a1c91a859a 75 if ( S & 0x80 )
gke 0:62a1c91a859a 76 L47 |= M;
gke 0:62a1c91a859a 77 else
gke 0:62a1c91a859a 78 L47 |= LOFF;
gke 0:62a1c91a859a 79 S <<= 1;
gke 0:62a1c91a859a 80 }
gke 0:62a1c91a859a 81
gke 0:62a1c91a859a 82 I2CLED.start();
gke 2:90292f8bd179 83 if ( I2CLED.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
gke 2:90292f8bd179 84 if ( I2CLED.write(0x15) != I2C_ACK ) goto PCA9551Error;
gke 2:90292f8bd179 85 if ( I2CLED.write(L03) != I2C_ACK ) goto PCA9551Error;
gke 2:90292f8bd179 86 if ( I2CLED.write(L47) != I2C_ACK ) goto PCA9551Error;
gke 0:62a1c91a859a 87 I2CLED.stop();
gke 0:62a1c91a859a 88
gke 2:90292f8bd179 89 return;
gke 2:90292f8bd179 90
gke 2:90292f8bd179 91 PCA9551Error:
gke 2:90292f8bd179 92 I2CLED.stop();
gke 2:90292f8bd179 93
gke 2:90292f8bd179 94 I2CError[PCA9551_ID]++;
gke 2:90292f8bd179 95
gke 0:62a1c91a859a 96 } // WritePCA9551
gke 0:62a1c91a859a 97
gke 0:62a1c91a859a 98 void SendLEDs(void) { // 39.3 uS @ 40MHz
gke 0:62a1c91a859a 99 static uint8 PCA9551LEDShadow;
gke 0:62a1c91a859a 100
gke 0:62a1c91a859a 101 if ( LEDShadow != PrevLEDShadow ) {
gke 0:62a1c91a859a 102
gke 0:62a1c91a859a 103 BlueLED = LEDShadow & BlueM;
gke 0:62a1c91a859a 104 RedLED = LEDShadow & RedM;
gke 0:62a1c91a859a 105 GreenLED = LEDShadow & GreenM;
gke 0:62a1c91a859a 106 YellowLED = LEDShadow & YellowM;
gke 0:62a1c91a859a 107
gke 0:62a1c91a859a 108 PCA9551LEDShadow = uint8(LEDShadow & 0x00ff);
gke 0:62a1c91a859a 109
gke 0:62a1c91a859a 110 if ( F.UsingLEDDriver && ( PCA9551LEDShadow != PrevPCA9551LEDShadow ) ) {
gke 0:62a1c91a859a 111 WritePCA9551(PCA9551LEDShadow);
gke 0:62a1c91a859a 112 PrevPCA9551LEDShadow = PCA9551LEDShadow;
gke 0:62a1c91a859a 113 }
gke 0:62a1c91a859a 114
gke 0:62a1c91a859a 115 PrevLEDShadow = LEDShadow;
gke 0:62a1c91a859a 116 }
gke 0:62a1c91a859a 117
gke 0:62a1c91a859a 118 } // SendLEDs
gke 0:62a1c91a859a 119
gke 0:62a1c91a859a 120
gke 0:62a1c91a859a 121 void SaveLEDs(void) { // one level only
gke 0:62a1c91a859a 122 SavedLEDs = LEDShadow;
gke 0:62a1c91a859a 123 } // SaveLEDs
gke 0:62a1c91a859a 124
gke 0:62a1c91a859a 125 void RestoreLEDs(void) {
gke 0:62a1c91a859a 126 LEDShadow = SavedLEDs;
gke 0:62a1c91a859a 127 SendLEDs();
gke 0:62a1c91a859a 128 } // RestoreLEDs
gke 0:62a1c91a859a 129
gke 0:62a1c91a859a 130 void LEDsOn(uint16 l) {
gke 0:62a1c91a859a 131 LEDShadow |= l;
gke 0:62a1c91a859a 132 SendLEDs();
gke 0:62a1c91a859a 133 } // LEDsOn
gke 0:62a1c91a859a 134
gke 0:62a1c91a859a 135 void LEDsOff(uint16 l) {
gke 0:62a1c91a859a 136 LEDShadow &= ~l;
gke 0:62a1c91a859a 137 SendLEDs();
gke 0:62a1c91a859a 138 } // LEDsOff
gke 0:62a1c91a859a 139
gke 0:62a1c91a859a 140 void LEDChaser(void) {
gke 0:62a1c91a859a 141 #define LED_NO (uint8)6 // skip buzzer
gke 0:62a1c91a859a 142 //#define LED_NO (uint8)7 // all LEDs
gke 0:62a1c91a859a 143
gke 0:62a1c91a859a 144 if ( mSClock() > mS[LEDChaserUpdate] ) {
gke 0:62a1c91a859a 145 if ( F.HoldingAlt ) {
gke 0:62a1c91a859a 146 LEDShadow ^= LEDChase[LEDPattern];
gke 0:62a1c91a859a 147 if ( LEDPattern < LED_NO ) LEDPattern++;
gke 0:62a1c91a859a 148 else LEDPattern = 0;
gke 0:62a1c91a859a 149 LEDShadow |= LEDChase[LEDPattern];
gke 0:62a1c91a859a 150 SendLEDs();
gke 0:62a1c91a859a 151 } else {
gke 0:62a1c91a859a 152 LEDShadow = SavedLEDs;
gke 0:62a1c91a859a 153 SendLEDs();
gke 0:62a1c91a859a 154 }
gke 0:62a1c91a859a 155
gke 0:62a1c91a859a 156 mS[LEDChaserUpdate] = mSClock() + 50;
gke 0:62a1c91a859a 157 }
gke 0:62a1c91a859a 158 } // LEDChaser
gke 0:62a1c91a859a 159
gke 0:62a1c91a859a 160 void DoLEDs(void) {
gke 0:62a1c91a859a 161 if ( F.AccelerationsValid ) LEDYellow_ON;
gke 0:62a1c91a859a 162 else LEDYellow_OFF;
gke 0:62a1c91a859a 163
gke 0:62a1c91a859a 164 if ( F.Signal ) {
gke 0:62a1c91a859a 165 LEDRed_OFF;
gke 0:62a1c91a859a 166 LEDGreen_ON;
gke 0:62a1c91a859a 167 } else {
gke 0:62a1c91a859a 168 LEDGreen_OFF;
gke 0:62a1c91a859a 169 LEDRed_ON;
gke 0:62a1c91a859a 170 }
gke 0:62a1c91a859a 171 } // DoLEDs
gke 0:62a1c91a859a 172
gke 0:62a1c91a859a 173 void PowerOutput(int8 d) {
gke 0:62a1c91a859a 174 int8 s;
gke 0:62a1c91a859a 175 uint16 m;
gke 0:62a1c91a859a 176
gke 0:62a1c91a859a 177 m = 1 << d;
gke 0:62a1c91a859a 178 for ( s=0; s < 10; s++ ) { // 10 flashes (count MUST be even!)
gke 0:62a1c91a859a 179 LEDShadow ^= m;
gke 0:62a1c91a859a 180 SendLEDs();
gke 0:62a1c91a859a 181 Delay1mS(50);
gke 0:62a1c91a859a 182 }
gke 0:62a1c91a859a 183 } // PowerOutput
gke 0:62a1c91a859a 184
gke 0:62a1c91a859a 185 void LEDsAndBuzzer(void) {
gke 0:62a1c91a859a 186 int8 s, m;
gke 0:62a1c91a859a 187 uint16 mask, LEDSave;
gke 0:62a1c91a859a 188
gke 0:62a1c91a859a 189 LEDSave = LEDShadow;
gke 0:62a1c91a859a 190 LEDShadow = 0;
gke 0:62a1c91a859a 191 SendLEDs();
gke 0:62a1c91a859a 192
gke 0:62a1c91a859a 193 TxString("\r\nOutput test\r\n");
gke 0:62a1c91a859a 194 TxString("Sequence Drv0/Buzz, Drv1, Drv2, Drv3, Aux0, Aux1, Aux2, Aux3, Y, R, G, B\r\n");
gke 0:62a1c91a859a 195 mask = (uint8)1;
gke 1:1e3318a30ddd 196 for ( m = 1; m <= 12; m++ ) {
gke 0:62a1c91a859a 197 for ( s = 0; s < 10; s++ ) { // 10 flashes (count MUST be even!)
gke 0:62a1c91a859a 198 LEDShadow ^= mask;
gke 0:62a1c91a859a 199 SendLEDs();
gke 0:62a1c91a859a 200 Delay1mS(100);
gke 0:62a1c91a859a 201 }
gke 0:62a1c91a859a 202 mask <<= 1;
gke 0:62a1c91a859a 203 }
gke 0:62a1c91a859a 204 LEDShadow = LEDSave;
gke 0:62a1c91a859a 205 SendLEDs();
gke 0:62a1c91a859a 206 TxString("Test Finished\r\n");
gke 0:62a1c91a859a 207 } // LEDsAndBuzzer
gke 0:62a1c91a859a 208
gke 0:62a1c91a859a 209 //______________________________________________________________________________________________
gke 0:62a1c91a859a 210
gke 0:62a1c91a859a 211 // LED Driver
gke 0:62a1c91a859a 212
gke 0:62a1c91a859a 213 void PCA9551Test(void) {
gke 2:90292f8bd179 214 static char b[8];
gke 2:90292f8bd179 215 static uint8 i;
gke 0:62a1c91a859a 216
gke 0:62a1c91a859a 217 TxString("\r\nPCA9551Test\r\n");
gke 0:62a1c91a859a 218
gke 0:62a1c91a859a 219 I2CLED.start();
gke 2:90292f8bd179 220 if ( I2CGYRO.write(PCA9551_ID) != I2C_ACK ) goto PCA9551Error;
gke 2:90292f8bd179 221 if ( I2CGYRO.write(0x11) != I2C_ACK ) goto PCA9551Error;;
gke 0:62a1c91a859a 222 I2CLED.stop();
gke 0:62a1c91a859a 223
gke 2:90292f8bd179 224 if ( I2CLED.blockread(PCA9551_ID, b, 7) ) goto PCA9551Error;
gke 0:62a1c91a859a 225
gke 0:62a1c91a859a 226 TxString("0:\t0b");
gke 0:62a1c91a859a 227 TxBin8(b[6]);
gke 0:62a1c91a859a 228 TxNextLine();
gke 0:62a1c91a859a 229 for (i = 0; i <6; i++ ) {
gke 0:62a1c91a859a 230 TxVal32(i+1, 0,0);
gke 0:62a1c91a859a 231 TxString(":\t0b");
gke 0:62a1c91a859a 232 TxBin8(b[i]);
gke 0:62a1c91a859a 233 TxNextLine();
gke 0:62a1c91a859a 234 }
gke 2:90292f8bd179 235
gke 2:90292f8bd179 236 TxNextLine();
gke 2:90292f8bd179 237
gke 2:90292f8bd179 238 return;
gke 2:90292f8bd179 239
gke 2:90292f8bd179 240 PCA9551Error:
gke 2:90292f8bd179 241 I2CLED.stop();
gke 2:90292f8bd179 242
gke 2:90292f8bd179 243 I2CError[PCA9551_ID]++;
gke 2:90292f8bd179 244
gke 1:1e3318a30ddd 245 TxString("FAILED\r\n");
gke 2:90292f8bd179 246
gke 0:62a1c91a859a 247 } // PCA9551Test
gke 0:62a1c91a859a 248
gke 0:62a1c91a859a 249 boolean IsPCA9551Active(void) {
gke 0:62a1c91a859a 250
gke 0:62a1c91a859a 251 const char b[7] = {0x11,0x25,0x80,0x25,0x80,0x00,0x00}; // Period 1Sec., PWM 50%, ON
gke 0:62a1c91a859a 252
gke 1:1e3318a30ddd 253 F.UsingLEDDriver = I2CGYROAddressResponds( PCA9551_ID );
gke 0:62a1c91a859a 254
gke 1:1e3318a30ddd 255 if ( F.UsingLEDDriver ) {
gke 2:90292f8bd179 256 if ( I2CLED.blockwrite(PCA9551_ID, b, 7) ) goto PCA9551Error;
gke 1:1e3318a30ddd 257 TrackMinI2CRate(400000);
gke 1:1e3318a30ddd 258 }
gke 0:62a1c91a859a 259
gke 2:90292f8bd179 260 #ifdef DISABLE_LED_DRIVER
gke 2:90292f8bd179 261 F.UsingLEDDriver = false;
gke 2:90292f8bd179 262 #endif // DISABLE_LED_DRIVER
gke 2:90292f8bd179 263
gke 1:1e3318a30ddd 264 return ( F.UsingLEDDriver );
gke 2:90292f8bd179 265
gke 2:90292f8bd179 266 PCA9551Error:
gke 2:90292f8bd179 267
gke 2:90292f8bd179 268 F.UsingLEDDriver = false;
gke 2:90292f8bd179 269
gke 2:90292f8bd179 270 return ( F.UsingLEDDriver );
gke 2:90292f8bd179 271
gke 0:62a1c91a859a 272 } //IsPCA9551Active
gke 0:62a1c91a859a 273
gke 0:62a1c91a859a 274 void InitLEDs(void) {
gke 0:62a1c91a859a 275
gke 0:62a1c91a859a 276 r = IsPCA9551Active();
gke 1:1e3318a30ddd 277
gke 0:62a1c91a859a 278 LEDShadow = SavedLEDs = LEDPattern = 0;
gke 0:62a1c91a859a 279 PrevLEDShadow = 0x0fff;
gke 0:62a1c91a859a 280
gke 0:62a1c91a859a 281 PrevPCA9551LEDShadow = 0;
gke 0:62a1c91a859a 282 WritePCA9551( PrevPCA9551LEDShadow );
gke 0:62a1c91a859a 283
gke 0:62a1c91a859a 284 } // InitLEDs
gke 0:62a1c91a859a 285