Prof Greg Egan
/
UAVXArm-GKE
UAVX Multicopter Flight Controller.
irq.c@1:1e3318a30ddd, 2011-02-25 (annotated)
- Committer:
- gke
- Date:
- Fri Feb 25 01:35:24 2011 +0000
- Revision:
- 1:1e3318a30ddd
- Parent:
- 0:62a1c91a859a
- Child:
- 2:90292f8bd179
This version has broken I2C - posted for debugging involvement of Simon et al.
Who changed what in which revision?
User | Revision | Line number | New 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 | 0:62a1c91a859a | 5 | // = http://code.google.com/p/uavp-mods/ http://uavp.ch = |
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 | // Interrupt Routines |
gke | 0:62a1c91a859a | 24 | |
gke | 0:62a1c91a859a | 25 | |
gke | 0:62a1c91a859a | 26 | const int16 MIN_PPM_SYNC_PAUSE = 2400; // uS |
gke | 0:62a1c91a859a | 27 | |
gke | 0:62a1c91a859a | 28 | // no less than 1500 |
gke | 0:62a1c91a859a | 29 | |
gke | 0:62a1c91a859a | 30 | extern void InitTimersAndInterrupts(void); |
gke | 0:62a1c91a859a | 31 | |
gke | 0:62a1c91a859a | 32 | void enableTxIrq0(void); |
gke | 0:62a1c91a859a | 33 | void disableTxIrq0(void); |
gke | 0:62a1c91a859a | 34 | void enableTxIrq1(void); |
gke | 0:62a1c91a859a | 35 | void disableTxIrq1(void); |
gke | 0:62a1c91a859a | 36 | |
gke | 0:62a1c91a859a | 37 | void RCISR(void); |
gke | 0:62a1c91a859a | 38 | void RCNullISR(void); |
gke | 0:62a1c91a859a | 39 | void RCTimeoutISR(void); |
gke | 0:62a1c91a859a | 40 | void CamPWMOnISR(void); |
gke | 0:62a1c91a859a | 41 | void CamRollPWMOffISR(void); |
gke | 0:62a1c91a859a | 42 | void CamPitchPWMOffISR(void); |
gke | 0:62a1c91a859a | 43 | void TelemetryInISR(void); |
gke | 0:62a1c91a859a | 44 | void GPSInISR(void); |
gke | 0:62a1c91a859a | 45 | |
gke | 0:62a1c91a859a | 46 | Timer timer; |
gke | 0:62a1c91a859a | 47 | Timeout CamRollTimeout, CamPitchTimeout; |
gke | 0:62a1c91a859a | 48 | Ticker CameraTicker; |
gke | 0:62a1c91a859a | 49 | Timeout RCTimeout; |
gke | 0:62a1c91a859a | 50 | |
gke | 0:62a1c91a859a | 51 | uint32 mS[CompassUpdate + 1]; |
gke | 0:62a1c91a859a | 52 | |
gke | 0:62a1c91a859a | 53 | uint32 PrevEdge, CurrEdge; |
gke | 0:62a1c91a859a | 54 | uint8 Intersection, PrevPattern, CurrPattern; |
gke | 0:62a1c91a859a | 55 | uint32 Width; |
gke | 0:62a1c91a859a | 56 | int16 PPM[MAX_CONTROLS]; |
gke | 0:62a1c91a859a | 57 | int8 PPM_Index; |
gke | 0:62a1c91a859a | 58 | int24 PauseTime; |
gke | 0:62a1c91a859a | 59 | uint8 RxState; |
gke | 0:62a1c91a859a | 60 | int8 State, FailState; |
gke | 0:62a1c91a859a | 61 | int8 SignalCount; |
gke | 0:62a1c91a859a | 62 | uint16 RCGlitches; |
gke | 0:62a1c91a859a | 63 | int16 PWMCycles = 0; |
gke | 0:62a1c91a859a | 64 | |
gke | 0:62a1c91a859a | 65 | void enableTxIrq0(void) { |
gke | 0:62a1c91a859a | 66 | LPC_UART0->IER |= 0x0002; |
gke | 0:62a1c91a859a | 67 | } // enableTxIrq0 |
gke | 0:62a1c91a859a | 68 | |
gke | 0:62a1c91a859a | 69 | void disableTxIrq0(void) { |
gke | 0:62a1c91a859a | 70 | LPC_UART0->IER &= ~0x0002; |
gke | 0:62a1c91a859a | 71 | } // disableTxIrq0 |
gke | 0:62a1c91a859a | 72 | |
gke | 0:62a1c91a859a | 73 | void enableTxIrq1(void) { |
gke | 0:62a1c91a859a | 74 | LPC_UART1->IER |= 0x0002; |
gke | 0:62a1c91a859a | 75 | } // enableTxIrq1 |
gke | 0:62a1c91a859a | 76 | |
gke | 0:62a1c91a859a | 77 | void disableTxIrq1(void) { |
gke | 0:62a1c91a859a | 78 | LPC_UART1->IER &= ~0x0002; |
gke | 0:62a1c91a859a | 79 | } // disableTxIrq1 |
gke | 0:62a1c91a859a | 80 | |
gke | 0:62a1c91a859a | 81 | void InitTimersAndInterrupts(void) { |
gke | 0:62a1c91a859a | 82 | static int8 i; |
gke | 0:62a1c91a859a | 83 | |
gke | 0:62a1c91a859a | 84 | timer.start(); |
gke | 0:62a1c91a859a | 85 | |
gke | 0:62a1c91a859a | 86 | GPSSerial.attach(GPSInISR, GPSSerial.RxIrq); |
gke | 0:62a1c91a859a | 87 | |
gke | 0:62a1c91a859a | 88 | InitTelemetryPacket(); |
gke | 0:62a1c91a859a | 89 | //TelemetrySerial.attach(TelemetryInISR, TelemetrySerial.RxIrq); |
gke | 0:62a1c91a859a | 90 | |
gke | 0:62a1c91a859a | 91 | RCPositiveEdge = true; |
gke | 0:62a1c91a859a | 92 | |
gke | 0:62a1c91a859a | 93 | RCTimeout.attach_us(& RCTimeoutISR, 50000); |
gke | 0:62a1c91a859a | 94 | |
gke | 0:62a1c91a859a | 95 | CameraTicker.attach_us(&CamPWMOnISR, 22500); |
gke | 0:62a1c91a859a | 96 | |
gke | 0:62a1c91a859a | 97 | for (i = Clock; i<= CompassUpdate; i++) |
gke | 0:62a1c91a859a | 98 | mS[i] = 0; |
gke | 0:62a1c91a859a | 99 | |
gke | 0:62a1c91a859a | 100 | } // InitTimersAndInterrupts |
gke | 0:62a1c91a859a | 101 | |
gke | 0:62a1c91a859a | 102 | void RCISR(void) { |
gke | 0:62a1c91a859a | 103 | CurrEdge = timer.read_us(); |
gke | 0:62a1c91a859a | 104 | |
gke | 0:62a1c91a859a | 105 | Width = CurrEdge - PrevEdge; |
gke | 0:62a1c91a859a | 106 | PrevEdge = CurrEdge; |
gke | 0:62a1c91a859a | 107 | |
gke | 0:62a1c91a859a | 108 | if ( Width > MIN_PPM_SYNC_PAUSE ) { // A pause > 5ms |
gke | 0:62a1c91a859a | 109 | PPM_Index = 0; // Sync pulse detected - next CH is CH1 |
gke | 0:62a1c91a859a | 110 | F.RCFrameOK = true; |
gke | 0:62a1c91a859a | 111 | F.RCNewValues = false; |
gke | 0:62a1c91a859a | 112 | PauseTime = Width; |
gke | 0:62a1c91a859a | 113 | } else |
gke | 0:62a1c91a859a | 114 | if (PPM_Index < RC_CONTROLS) { // Width in 1us ticks. |
gke | 0:62a1c91a859a | 115 | if ( ( Width >= RC_MIN_WIDTH_US ) && ( Width <= RC_MAX_WIDTH_US ) ) |
gke | 0:62a1c91a859a | 116 | PPM[PPM_Index] = Width - 1000; |
gke | 0:62a1c91a859a | 117 | else { |
gke | 0:62a1c91a859a | 118 | // preserve old value i.e. default hold |
gke | 0:62a1c91a859a | 119 | RCGlitches++; |
gke | 0:62a1c91a859a | 120 | F.RCFrameOK = false; |
gke | 0:62a1c91a859a | 121 | } |
gke | 0:62a1c91a859a | 122 | |
gke | 0:62a1c91a859a | 123 | PPM_Index++; |
gke | 0:62a1c91a859a | 124 | // MUST demand rock solid RC frames for autonomous functions not |
gke | 0:62a1c91a859a | 125 | // to be cancelled by noise-generated partially correct frames |
gke | 0:62a1c91a859a | 126 | if ( PPM_Index == RC_CONTROLS ) { |
gke | 0:62a1c91a859a | 127 | if ( F.RCFrameOK ) { |
gke | 0:62a1c91a859a | 128 | F.RCNewValues = true; |
gke | 0:62a1c91a859a | 129 | SignalCount++; |
gke | 0:62a1c91a859a | 130 | } else { |
gke | 0:62a1c91a859a | 131 | F.RCNewValues = false; |
gke | 0:62a1c91a859a | 132 | SignalCount -= RC_GOOD_RATIO; |
gke | 0:62a1c91a859a | 133 | } |
gke | 0:62a1c91a859a | 134 | |
gke | 0:62a1c91a859a | 135 | SignalCount = Limit(SignalCount, -RC_GOOD_BUCKET_MAX, RC_GOOD_BUCKET_MAX); |
gke | 0:62a1c91a859a | 136 | F.Signal = SignalCount > 0; |
gke | 0:62a1c91a859a | 137 | |
gke | 0:62a1c91a859a | 138 | if ( F.Signal ) |
gke | 0:62a1c91a859a | 139 | mS[LastValidRx] = timer.read_ms(); |
gke | 0:62a1c91a859a | 140 | |
gke | 0:62a1c91a859a | 141 | RCTimeout.attach_us(& RCTimeoutISR, RC_SIGNAL_TIMEOUT_MS*1000); |
gke | 0:62a1c91a859a | 142 | } |
gke | 0:62a1c91a859a | 143 | } |
gke | 0:62a1c91a859a | 144 | |
gke | 0:62a1c91a859a | 145 | } // RCISR |
gke | 0:62a1c91a859a | 146 | |
gke | 0:62a1c91a859a | 147 | void RCNullISR(void) { |
gke | 0:62a1c91a859a | 148 | // discard edge |
gke | 0:62a1c91a859a | 149 | } // RCNullISR |
gke | 0:62a1c91a859a | 150 | |
gke | 0:62a1c91a859a | 151 | void CamPWMOnISR(void) { |
gke | 0:62a1c91a859a | 152 | |
gke | 0:62a1c91a859a | 153 | PWMCamRoll.write(true); |
gke | 0:62a1c91a859a | 154 | CamRollTimeout.attach_us(&CamRollPWMOffISR, CamRollPulseWidth); |
gke | 0:62a1c91a859a | 155 | PWMCamPitch.write(true); |
gke | 0:62a1c91a859a | 156 | CamPitchTimeout.attach_us(&CamPitchPWMOffISR, CamPitchPulseWidth); |
gke | 0:62a1c91a859a | 157 | |
gke | 0:62a1c91a859a | 158 | } // CamPWMOnISR |
gke | 0:62a1c91a859a | 159 | |
gke | 0:62a1c91a859a | 160 | void CamRollPWMOffISR(void) { |
gke | 0:62a1c91a859a | 161 | |
gke | 0:62a1c91a859a | 162 | PWMCamRoll.write(false); |
gke | 0:62a1c91a859a | 163 | CamRollTimeout.detach(); |
gke | 0:62a1c91a859a | 164 | |
gke | 0:62a1c91a859a | 165 | } // CamRollPWMOffISR |
gke | 0:62a1c91a859a | 166 | |
gke | 0:62a1c91a859a | 167 | void CamPitchPWMOffISR(void) { |
gke | 0:62a1c91a859a | 168 | |
gke | 0:62a1c91a859a | 169 | PWMCamPitch.write(false); |
gke | 0:62a1c91a859a | 170 | CamPitchTimeout.detach(); |
gke | 0:62a1c91a859a | 171 | |
gke | 0:62a1c91a859a | 172 | } // CamPitchPWMOffISR |
gke | 0:62a1c91a859a | 173 | |
gke | 0:62a1c91a859a | 174 | void RCTimeoutISR(void) { |
gke | 0:62a1c91a859a | 175 | if ( F.Signal ) { |
gke | 0:62a1c91a859a | 176 | F.Signal = false; |
gke | 0:62a1c91a859a | 177 | SignalCount = -RC_GOOD_BUCKET_MAX; |
gke | 0:62a1c91a859a | 178 | } |
gke | 0:62a1c91a859a | 179 | } // RCTimeoutISR |
gke | 0:62a1c91a859a | 180 | |
gke | 0:62a1c91a859a | 181 | void GPSInISR(void) { |
gke | 0:62a1c91a859a | 182 | RxGPSPacket( GPSSerial.getc()); |
gke | 0:62a1c91a859a | 183 | } // GPSInISR |
gke | 0:62a1c91a859a | 184 | |
gke | 0:62a1c91a859a | 185 | void TelemetryInISR(void) { |
gke | 0:62a1c91a859a | 186 | RxTelemetryPacket(TelemetrySerial.getc()); |
gke | 0:62a1c91a859a | 187 | } // TelemetryInISR |
gke | 0:62a1c91a859a | 188 |