Rev 2109 | Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
2108 | - | 1 | #include <stdlib.h> |
2 | #include <avr/io.h> |
||
3 | #include <avr/interrupt.h> |
||
4 | |||
5 | #include "rc.h" |
||
6 | #include "controlMixer.h" |
||
7 | #include "configuration.h" |
||
8 | #include "commands.h" |
||
9 | #include "output.h" |
||
10 | |||
11 | // The channel array is 0-based! |
||
12 | volatile int16_t PPM_in[MAX_CHANNELS]; |
||
13 | volatile uint8_t RCQuality; |
||
14 | |||
15 | uint8_t lastRCCommand = COMMAND_NONE; |
||
16 | uint8_t lastFlightMode = FLIGHT_MODE_NONE; |
||
17 | |||
18 | /*************************************************************** |
||
19 | * 16bit timer 1 is used to decode the PPM-Signal |
||
20 | ***************************************************************/ |
||
21 | void RC_Init(void) { |
||
22 | uint8_t sreg = SREG; |
||
23 | |||
24 | // disable all interrupts before reconfiguration |
||
25 | cli(); |
||
26 | |||
27 | // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
||
28 | DDRD &= ~(1<<6); |
||
29 | PORTD |= (1<<PORTD6); |
||
30 | |||
31 | // Channel 5,6,7 is decoded to servo signals at pin PD5 (J3), PD4(J4), PD3(J5) |
||
32 | // set as output |
||
33 | DDRD |= (1<<DDD5) | (1<<DDD4) | (1<<DDD3); |
||
34 | // low level |
||
35 | PORTD &= ~((1<<PORTD5) | (1<<PORTD4) | (1<<PORTD3)); |
||
36 | |||
37 | // PD3 can't be used if 2nd UART is activated |
||
38 | // because TXD1 is at that port |
||
39 | // if (CPUType != ATMEGA644P) { |
||
40 | // DDRD |= (1<<PORTD3); |
||
41 | // PORTD &= ~(1<<PORTD3); |
||
42 | // } |
||
43 | |||
44 | // Timer/Counter1 Control Register A, B, C |
||
45 | // Normal Mode (bits: WGM13=0, WGM12=0, WGM11=0, WGM10=0) |
||
46 | // Compare output pin A & B is disabled (bits: COM1A1=0, COM1A0=0, COM1B1=0, COM1B0=0) |
||
47 | // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
||
48 | // Enable input capture noise cancler (bit: ICNC1=1) |
||
49 | // Trigger on positive edge of the input capture pin (bit: ICES1=1), |
||
50 | // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2�s |
||
51 | // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
||
52 | TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10)); |
||
53 | TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
||
54 | TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
||
55 | TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
||
56 | |||
57 | // Timer/Counter1 Interrupt Mask Register |
||
58 | // Enable Input Capture Interrupt (bit: ICIE1=1) |
||
59 | // Disable Output Compare A & B Match Interrupts (bit: OCIE1B=0, OICIE1A=0) |
||
60 | // Enable Overflow Interrupt (bit: TOIE1=0) |
||
61 | TIMSK1 &= ~((1<<OCIE1B) | (1<<OCIE1A) | (1<<TOIE1)); |
||
62 | TIMSK1 |= (1<<ICIE1); |
||
63 | |||
64 | RCQuality = 0; |
||
65 | |||
66 | SREG = sreg; |
||
67 | } |
||
68 | |||
69 | /********************************************************************/ |
||
70 | /* Every time a positive edge is detected at PD6 */ |
||
71 | /********************************************************************/ |
||
72 | /* t-Frame |
||
73 | <-----------------------------------------------------------------------> |
||
74 | ____ ______ _____ ________ ______ sync gap ____ |
||
75 | | | | | | | | | | | | |
||
76 | | | | | | | | | | | | |
||
77 | ___| |_| |_| |_| |_.............| |________________| |
||
78 | <-----><-------><------><----------- <------> <--- |
||
79 | t0 t1 t2 t4 tn t0 |
||
80 | |||
81 | The PPM-Frame length is 22.5 ms. |
||
82 | Channel high pulse width range is 0.7 ms to 1.7 ms completed by an 0.3 ms low pulse. |
||
83 | The mininimum time delay of two events coding a channel is ( 0.7 + 0.3) ms = 1 ms. |
||
84 | The maximum time delay of two events coding a channel is ( 1.7 + 0.3) ms = 2 ms. |
||
85 | The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
||
86 | The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
||
87 | The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
||
88 | the syncronization gap. |
||
89 | */ |
||
90 | ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
||
91 | int16_t signal = 0, tmp; |
||
92 | static int16_t index; |
||
93 | static uint16_t oldICR1 = 0; |
||
94 | |||
95 | // 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
||
96 | // at the time the edge was detected |
||
97 | |||
98 | // calculate the time delay to the previous event time which is stored in oldICR1 |
||
99 | // calculatiing the difference of the two uint16_t and converting the result to an int16_t |
||
100 | // implicit handles a timer overflow 65535 -> 0 the right way. |
||
101 | signal = (uint16_t) ICR1 - oldICR1; |
||
102 | oldICR1 = ICR1; |
||
103 | |||
104 | //sync gap? (3.52 ms < signal < 25.6 ms) |
||
105 | if ((signal > 1100) && (signal < 8000)) { |
||
106 | index = 0; |
||
107 | } else { // within the PPM frame |
||
108 | if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
||
109 | // check for valid signal length (0.8 ms < signal < 2.1984 ms) |
||
110 | // signal range is from 1.0ms/3.2us = 312 to 2.0ms/3.2us = 625 |
||
111 | if ((signal > 250) && (signal < 687)) { |
||
112 | // shift signal to zero symmetric range -154 to 159 |
||
113 | signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
||
114 | // check for stable signal |
||
115 | if (abs(signal - PPM_in[index]) < 6) { |
||
116 | if (RCQuality < 200) |
||
117 | RCQuality += 10; |
||
118 | else |
||
119 | RCQuality = 200; |
||
120 | } |
||
121 | // If signal is the same as before +/- 1, just keep it there. Naah lets get rid of this slimy sticy stuff. |
||
122 | // if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
||
123 | // In addition, if the signal is very close to 0, just set it to 0. |
||
124 | if (signal >= -1 && signal <= 1) { |
||
125 | tmp = 0; |
||
126 | //} else { |
||
127 | // tmp = PPM_in[index]; |
||
128 | // } |
||
129 | } else |
||
130 | tmp = signal; |
||
131 | PPM_in[index] = tmp; // update channel value |
||
132 | } |
||
133 | index++; // next channel |
||
134 | // demux sum signal for channels 5 to 7 to J3, J4, J5 |
||
135 | // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
||
136 | // channels are usually available at the receiver anyway. |
||
137 | // if(index == 5) J3HIGH; else J3LOW; |
||
138 | // if(index == 6) J4HIGH; else J4LOW; |
||
139 | // if(CPUType != ATMEGA644P) // not used as TXD1 |
||
140 | // { |
||
141 | // if(index == 7) J5HIGH; else J5LOW; |
||
142 | // } |
||
143 | } |
||
144 | } |
||
145 | } |
||
146 | |||
147 | #define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
||
148 | #define COMMAND_THRESHOLD 85 |
||
149 | #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
||
150 | #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
||
151 | |||
152 | #define RC_SCALING 4 |
||
153 | |||
154 | uint8_t getControlModeSwitch(void) { |
||
155 | int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET; |
||
156 | uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL : |
||
157 | (channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE); |
||
158 | return flightMode; |
||
159 | } |
||
160 | |||
161 | // Gyro calibration is performed as.... well mode switch with no throttle and no airspeed would be nice. |
||
162 | // Maybe simply: Very very low throttle. |
||
163 | // Throttle xlow for COMMAND_TIMER: GYROCAL (once). |
||
164 | // mode switched: CHMOD |
||
165 | |||
166 | uint8_t RC_getCommand(void) { |
||
167 | uint8_t flightMode = getControlModeSwitch(); |
||
168 | |||
169 | if (lastFlightMode != flightMode) { |
||
170 | lastFlightMode = flightMode; |
||
171 | lastRCCommand = COMMAND_CHMOD; |
||
172 | return lastRCCommand; |
||
173 | } |
||
174 | |||
175 | int16_t channel = RCChannel(CH_THROTTLE); |
||
176 | |||
177 | if (channel <= -140) { // <= 900 us |
||
178 | lastRCCommand = COMMAND_GYROCAL; |
||
179 | } else { |
||
180 | lastRCCommand = COMMAND_NONE; |
||
181 | } |
||
182 | return lastRCCommand; |
||
183 | } |
||
184 | |||
185 | uint8_t RC_getArgument(void) { |
||
186 | return lastFlightMode; |
||
187 | } |
||
188 | |||
189 | /* |
||
190 | * Get Pitch, Roll, Throttle, Yaw values |
||
191 | */ |
||
192 | void RC_periodicTaskAndPRYT(int16_t* PRYT) { |
||
193 | if (RCQuality) { |
||
194 | RCQuality--; |
||
195 | |||
196 | debugOut.analog[20] = RCChannel(CH_ELEVATOR); |
||
197 | debugOut.analog[21] = RCChannel(CH_AILERONS); |
||
198 | debugOut.analog[22] = RCChannel(CH_RUDDER); |
||
199 | debugOut.analog[23] = RCChannel(CH_THROTTLE); |
||
200 | |||
201 | PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING; |
||
202 | PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING; |
||
203 | PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING; |
||
204 | PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING; |
||
205 | } // if RCQuality is no good, we just do nothing. |
||
206 | } |
||
207 | |||
208 | /* |
||
209 | * Get other channel value |
||
210 | */ |
||
211 | int16_t RC_getVariable(uint8_t varNum) { |
||
212 | if (varNum < 4) |
||
213 | // 0th variable is 5th channel (1-based) etc. |
||
214 | return RCChannel(varNum + CH_POTS) + POT_OFFSET; |
||
215 | /* |
||
216 | * Let's just say: |
||
217 | * The RC variable i is hardwired to channel i, i>=4 |
||
218 | */ |
||
219 | return PPM_in[varNum] + POT_OFFSET; |
||
220 | } |
||
221 | |||
222 | uint8_t RC_getSignalQuality(void) { |
||
223 | if (RCQuality >= 160) |
||
224 | return SIGNAL_GOOD; |
||
225 | if (RCQuality >= 140) |
||
226 | return SIGNAL_OK; |
||
227 | if (RCQuality >= 120) |
||
228 | return SIGNAL_BAD; |
||
229 | return SIGNAL_LOST; |
||
230 | } |
||
231 | |||
232 | /* |
||
233 | * To should fired only when the right stick is in the center position. |
||
234 | * This will cause the value of pitch and roll stick to be adjusted |
||
235 | * to zero (not just to near zero, as per the assumption in rc.c |
||
236 | * about the rc signal. I had values about 50..70 with a Futaba |
||
237 | * R617 receiver.) This calibration is not strictly necessary, but |
||
238 | * for control logic that depends on the exact (non)center position |
||
239 | * of a stick, it may be useful. |
||
240 | */ |
||
241 | void RC_calibrate(void) { |
||
242 | // Do nothing. |
||
243 | } |