Prof Greg Egan / Mbed 2 deprecated UAVXArm-GKE

UAVX Multicopter Flight Controller.

Dependencies:   mbed

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?

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 // 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