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