irq.c

00001 // ===============================================================================================
00002 // =                              UAVXArm Quadrocopter Controller                                =
00003 // =                           Copyright (c) 2008 by Prof. Greg Egan                             =
00004 // =                 Original V3.15 Copyright (c) 2007 Ing. Wolfgang Mahringer                   =
00005 // =                           http://code.google.com/p/uavp-mods/                               =
00006 // ===============================================================================================
00007 
00008 //    This is part of UAVXArm.
00009 
00010 //    UAVXArm is free software: you can redistribute it and/or modify it under the terms of the GNU
00011 //    General Public License as published by the Free Software Foundation, either version 3 of the
00012 //    License, or (at your option) any later version.
00013 
00014 //    UAVXArm is distributed in the hope that it will be useful,but WITHOUT ANY WARRANTY; without
00015 //    even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
00016 //    See the GNU General Public License for more details.
00017 
00018 //    You should have received a copy of the GNU General Public License along with this program.
00019 //    If not, see http://www.gnu.org/licenses/
00020 
00021 #include "UAVXArm.h"
00022 
00023 // Interrupt Routines
00024 
00025 const int16 MIN_PPM_SYNC_PAUSE = 2400;      // uS
00026 
00027 // no less than 1500
00028 
00029 extern void InitTimersAndInterrupts(void);
00030 
00031 void enableTxIrq0(void);
00032 void disableTxIrq0(void);
00033 void enableTxIrq1(void);
00034 void disableTxIrq1(void);
00035 
00036 void RCISR(void);
00037 void RCNullISR(void);
00038 void RCTimeoutISR(void);
00039 void CamPWMOnISR(void);
00040 void CamRollPWMOffISR(void);
00041 void CamPitchPWMOffISR(void);
00042 void TelemetryInISR(void);
00043 void GPSInISR(void);
00044 
00045 Timer timer;
00046 Timeout CamRollTimeout, CamPitchTimeout;
00047 Ticker CameraTicker;
00048 Timeout RCTimeout;
00049 
00050 uint32    mS[CompassUpdate + 1];
00051 
00052 uint32    PrevEdge, CurrEdge;
00053 uint8     Intersection, PrevPattern, CurrPattern;
00054 uint32    Width;
00055 int16     PPM[MAX_CONTROLS];
00056 int8      PPM_Index;
00057 int24     PauseTime;
00058 uint8     RxState;
00059 int8      State, FailState;
00060 int8      SignalCount;
00061 uint16    RCGlitches;
00062 int16     PWMCycles = 0;
00063 
00064 void InitTimersAndInterrupts(void) {
00065     static int8 i;
00066 
00067     timer.start();
00068 
00069     GPSSerial.attach(GPSInISR, GPSSerial.RxIrq);
00070 
00071     InitTelemetryPacket();
00072     //TelemetrySerial.attach(TelemetryInISR, TelemetrySerial.RxIrq);
00073 
00074     RCPositiveEdge = true;
00075 
00076     RCTimeout.attach_us(& RCTimeoutISR, 50000);
00077 
00078     CameraTicker.attach_us(&CamPWMOnISR, 22500);
00079 
00080     for (i = Clock; i<= CompassUpdate; i++)
00081         mS[i] = 0;
00082 
00083 } // InitTimersAndInterrupts
00084 
00085 void RCISR(void) {
00086     CurrEdge = timer.read_us();
00087 
00088     Width = CurrEdge - PrevEdge;
00089     PrevEdge = CurrEdge;
00090 
00091     if ( Width > MIN_PPM_SYNC_PAUSE ) {  // A pause  > 5ms
00092         PPM_Index = 0;                        // Sync pulse detected - next CH is CH1
00093         F.RCFrameOK = true;
00094         F.RCNewValues = false;
00095         PauseTime = Width;
00096     } else
00097         if (PPM_Index < RC_CONTROLS) { // Width in 1us ticks.
00098             if ( ( Width >= RC_MIN_WIDTH_US ) && ( Width <= RC_MAX_WIDTH_US ) )
00099                 PPM[PPM_Index] = Width - 1000;
00100             else {
00101                 // preserve old value i.e. default hold
00102                 RCGlitches++;
00103                 F.RCFrameOK = false;
00104             }
00105 
00106             PPM_Index++;
00107             // MUST demand rock solid RC frames for autonomous functions not
00108             // to be cancelled by noise-generated partially correct frames
00109             if ( PPM_Index == RC_CONTROLS ) {
00110                 if ( F.RCFrameOK ) {
00111                     F.RCNewValues = true;
00112                     SignalCount++;
00113                 } else {
00114                     F.RCNewValues = false;
00115                     SignalCount -= RC_GOOD_RATIO;
00116                 }
00117 
00118                 SignalCount = Limit(SignalCount, -RC_GOOD_BUCKET_MAX, RC_GOOD_BUCKET_MAX);
00119                 F.Signal = SignalCount > 0;
00120 
00121                 if ( F.Signal )
00122                     mS[LastValidRx] = timer.read_ms();
00123 
00124                 RCTimeout.attach_us(& RCTimeoutISR, RC_SIGNAL_TIMEOUT_MS*1000);
00125             }
00126         }
00127 
00128 } // RCISR
00129 
00130 void RCNullISR(void) {
00131 // discard edge
00132 } // RCNullISR
00133 
00134 void CamPWMOnISR(void) {
00135 
00136     PWMCamRoll.write(true);
00137     CamRollTimeout.attach_us(&CamRollPWMOffISR, CamRollPulseWidth);
00138     PWMCamPitch.write(true);
00139     CamPitchTimeout.attach_us(&CamPitchPWMOffISR, CamPitchPulseWidth);
00140 
00141 } // CamPWMOnISR
00142 
00143 void CamRollPWMOffISR(void) {
00144 
00145     PWMCamRoll.write(false);
00146     CamRollTimeout.detach();
00147 
00148 } // CamRollPWMOffISR
00149 
00150 void CamPitchPWMOffISR(void) {
00151 
00152     PWMCamPitch.write(false);
00153     CamPitchTimeout.detach();
00154 
00155 } // CamPitchPWMOffISR
00156 
00157 void RCTimeoutISR(void) {
00158     if ( F.Signal ) {
00159         F.Signal = false;
00160         SignalCount = -RC_GOOD_BUCKET_MAX;
00161     }
00162 } // RCTimeoutISR
00163 
00164 void GPSInISR(void) {
00165     RxGPSPacket( GPSSerial.getc());
00166 } // GPSInISR
00167 
00168 void TelemetryInISR(void) {
00169     RxTelemetryPacket(TelemetrySerial.getc());
00170 } // TelemetryInISR
00171