Skip to content

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP
Newer
Older
100644 161 lines (144 sloc) 6.872 kb
2d625bb git-svn-id: http://multiwii.googlecode.com/svn/trunk/MultiWii@168 02679b...
dubusal@gmail.com authored
1 static uint8_t pinRcChannel[8] = {ROLLPIN, PITCHPIN, YAWPIN, THROTTLEPIN, AUX1PIN,AUX2PIN,CAM1PIN,CAM2PIN};
2 volatile uint16_t rcPinValue[8] = {1500,1500,1500,1500,1500,1500,1500,1500}; // interval [1000;2000]
3 static int16_t rcData4Values[8][4];
4 static int16_t rcDataMean[8] ;
5
6 // ***PPM SUM SIGNAL***
7 #ifdef SERIAL_SUM_PPM
8 static uint8_t rcChannel[8] = {SERIAL_SUM_PPM};
9 #endif
10 volatile uint16_t rcValue[8] = {1500,1500,1500,1500,1500,1500,1500,1500}; // interval [1000;2000]
11
12 // Configure each rc pin for PCINT
13 void configureReceiver() {
14 #if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM)
15 for (uint8_t chan = 0; chan < 8; chan++)
16 for (uint8_t a = 0; a < 4; a++)
17 rcData4Values[chan][a] = 1500; //we initiate the default value of each channel. If there is no RC receiver connected, we will see those values
18 #if defined(PROMINI)
19 // PCINT activated only for specific pin inside [D0-D7] , [D2 D4 D5 D6 D7] for this multicopter
20 PORTD = (1<<2) | (1<<4) | (1<<5) | (1<<6) | (1<<7); //enable internal pull ups on the PINs of PORTD (no high impedence PINs)
21 PCMSK2 |= (1<<2) | (1<<4) | (1<<5) | (1<<6) | (1<<7);
22 PCICR = 1<<2; // PCINT activated only for the port dealing with [D0-D7] PINs
23 #endif
24 #if defined(MEGA)
25 // PCINT activated only for specific pin inside [A8-A15]
26 DDRK = 0; // defined PORTK as a digital port ([A8-A15] are consired as digital PINs and not analogical)
27 PORTK = (1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5) | (1<<6) | (1<<7); //enable internal pull ups on the PINs of PORTK
28 PCMSK2 |= (1<<0) | (1<<1) | (1<<2) | (1<<3) | (1<<4) | (1<<5) | (1<<6) | (1<<7);
29 PCICR = 1<<2; // PCINT activated only for PORTK dealing with [A8-A15] PINs
30 #endif
31 #endif
32 #if defined(SERIAL_SUM_PPM)
33 PPM_PIN_INTERRUPT
34 #endif
35 #if defined (SPEKTRUM)
36
37 #endif
38 }
39
40 #if !defined(SERIAL_SUM_PPM) && !defined(SPEKTRUM)
41 ISR(PCINT2_vect) { //this ISR is common to every receiver channel, it is call everytime a change state occurs on a digital pin [D2-D7]
42 uint8_t mask;
43 uint8_t pin;
44 uint16_t cTime,dTime;
45 static uint16_t edgeTime[8];
46 static uint8_t PCintLast;
47
48 #if defined(PROMINI)
49 pin = PIND; // PIND indicates the state of each PIN for the arduino port dealing with [D0-D7] digital pins (8 bits variable)
50 #endif
51 #if defined(MEGA)
52 pin = PINK; // PINK indicates the state of each PIN for the arduino port dealing with [A8-A15] digital pins (8 bits variable)
53 #endif
54 mask = pin ^ PCintLast; // doing a ^ between the current interruption and the last one indicates wich pin changed
55 sei(); // re enable other interrupts at this point, the rest of this interrupt is not so time critical and can be interrupted safely
56 PCintLast = pin; // we memorize the current state of all PINs [D0-D7]
57
58 cTime = micros(); // micros() return a uint32_t, but it is not usefull to keep the whole bits => we keep only 16 bits
59
60 // mask is pins [D0-D7] that have changed // the principle is the same on the MEGA for PORTK and [A8-A15] PINs
61 // chan = pin sequence of the port. chan begins at D2 and ends at D7
62 if (mask & 1<<2) //indicates the bit 2 of the arduino port [D0-D7], that is to say digital pin 2, if 1 => this pin has just changed
63 if (!(pin & 1<<2)) { //indicates if the bit 2 of the arduino port [D0-D7] is not at a high state (so that we match here only descending PPM pulse)
64 dTime = cTime-edgeTime[2]; if (900<dTime && dTime<2200) rcPinValue[2] = dTime; // just a verification: the value must be in the range [1000;2000] + some margin
65 } else edgeTime[2] = cTime; // if the bit 2 of the arduino port [D0-D7] is at a high state (ascending PPM pulse), we memorize the time
66 if (mask & 1<<4) //same principle for other channels // avoiding a for() is more than twice faster, and it's important to minimize execution time in ISR
67 if (!(pin & 1<<4)) {
68 dTime = cTime-edgeTime[4]; if (900<dTime && dTime<2200) rcPinValue[4] = dTime;
69 } else edgeTime[4] = cTime;
70 if (mask & 1<<5)
71 if (!(pin & 1<<5)) {
72 dTime = cTime-edgeTime[5]; if (900<dTime && dTime<2200) rcPinValue[5] = dTime;
73 } else edgeTime[5] = cTime;
74 if (mask & 1<<6)
75 if (!(pin & 1<<6)) {
76 dTime = cTime-edgeTime[6]; if (900<dTime && dTime<2200) rcPinValue[6] = dTime;
77 } else edgeTime[6] = cTime;
78 if (mask & 1<<7)
79 if (!(pin & 1<<7)) {
80 dTime = cTime-edgeTime[7]; if (900<dTime && dTime<2200) rcPinValue[7] = dTime;
81 } else edgeTime[7] = cTime;
82 #if defined(MEGA)
83 if (mask & 1<<0)
84 if (!(pin & 1<<0)) {
85 dTime = cTime-edgeTime[0]; if (900<dTime && dTime<2200) rcPinValue[0] = dTime;
86 } else edgeTime[0] = cTime;
87 if (mask & 1<<1)
88 if (!(pin & 1<<1)) {
89 dTime = cTime-edgeTime[1]; if (900<dTime && dTime<2200) rcPinValue[1] = dTime;
90 } else edgeTime[1] = cTime;
91 if (mask & 1<<3)
92 if (!(pin & 1<<3)) {
93 dTime = cTime-edgeTime[3]; if (900<dTime && dTime<2200) rcPinValue[3] = dTime;
94 } else edgeTime[3] = cTime;
95 #endif
96 #if defined(FAILSAFE)
97 if (mask & 1<<THROTTLEPIN) { // If pulse present on THROTTLE pin (independent from ardu version), clear FailSafe counter - added by MIS
98 if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; }
99 #endif
100 }
101 #endif
102
103 #if defined(SERIAL_SUM_PPM)
104 void rxInt() {
105 uint16_t now,diff;
106 static uint16_t last = 0;
107 static uint8_t chan = 0;
108
109 now = micros();
110 diff = now - last;
111 last = now;
112 if(diff>3000) chan = 0;
113 else {
114 if(900<diff && diff<2200 && chan<8) rcValue[chan] = diff;
115 chan++;
116 #if defined(FAILSAFE)
117 if(failsafeCnt > 20) failsafeCnt -= 20; else failsafeCnt = 0; // clear FailSafe counter - added by MIS
118 #endif
119 }
120 }
121 #endif
122
123 #if defined(SPEKTRUM)
124
125
126 #endif
127
128 uint16_t readRawRC(uint8_t chan) {
129 uint16_t data;
130 uint8_t oldSREG;
131 oldSREG = SREG;
132 cli(); // Let's disable interrupts
133 #ifndef SERIAL_SUM_PPM
134 data = rcPinValue[pinRcChannel[chan]]; // Let's copy the data Atomically
135 #else
136 data = rcValue[rcChannel[chan]];
137 #endif
138 SREG = oldSREG;
139 sei();// Let's enable the interrupts
140 #if defined(PROMINI) && !defined(SERIAL_SUM_PPM)
141 if (chan>4) return 1500;
142 #endif
143 return data; // We return the value correctly copied when the IRQ's where disabled
144 }
145
146 void computeRC() {
147 static uint8_t rc4ValuesIndex = 0;
148 uint8_t chan,a;
149
150 rc4ValuesIndex++;
151 for (chan = 0; chan < 8; chan++) {
152 rcData4Values[chan][rc4ValuesIndex%4] = readRawRC(chan);
153 rcDataMean[chan] = 0;
154 for (a=0;a<4;a++) rcDataMean[chan] += rcData4Values[chan][a];
155 rcDataMean[chan]= (rcDataMean[chan]+2)/4;
156 if ( rcDataMean[chan] < rcData[chan] -3) rcData[chan] = rcDataMean[chan]+2;
157 if ( rcDataMean[chan] > rcData[chan] +3) rcData[chan] = rcDataMean[chan]-2;
158 }
159 }
160
Something went wrong with that request. Please try again.