Rev 2108 | Rev 2110 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2108 | Rev 2109 | ||
---|---|---|---|
Line 13... | Line 13... | ||
13 | volatile uint8_t RCQuality; |
13 | volatile uint8_t RCQuality; |
Line 14... | Line 14... | ||
14 | 14 | ||
15 | uint8_t lastRCCommand = COMMAND_NONE; |
15 | uint8_t lastRCCommand = COMMAND_NONE; |
Line -... | Line 16... | ||
- | 16 | uint8_t lastFlightMode = FLIGHT_MODE_NONE; |
|
- | 17 | ||
16 | uint8_t lastFlightMode = FLIGHT_MODE_NONE; |
18 | #define TIME(s) ((int16_t)(((long)F_CPU/(long)8000)*(float)s)) |
17 | 19 | ||
18 | /*************************************************************** |
20 | /*************************************************************** |
19 | * 16bit timer 1 is used to decode the PPM-Signal |
21 | * 16bit timer 1 is used to decode the PPM-Signal |
20 | ***************************************************************/ |
22 | ***************************************************************/ |
Line 21... | Line 23... | ||
21 | void RC_Init(void) { |
23 | void RC_Init(void) { |
22 | uint8_t sreg = SREG; |
24 | uint8_t sreg = SREG; |
Line 23... | Line 25... | ||
23 | 25 | ||
24 | // disable all interrupts before reconfiguration |
26 | // disable all interrupts before reconfiguration |
25 | cli(); |
27 | 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 |
- | |
Line 39... | Line 28... | ||
39 | // if (CPUType != ATMEGA644P) { |
28 | |
40 | // DDRD |= (1<<PORTD3); |
29 | // PPM-signal is connected to the Input Capture Pin (PD6) of timer 1 |
41 | // PORTD &= ~(1<<PORTD3); |
30 | DDRB &= ~(1<<0); |
42 | // } |
31 | PORTB |= (1<<PORTB0); |
Line 49... | Line 38... | ||
49 | // Trigger on positive edge of the input capture pin (bit: ICES1=1), |
38 | // 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 |
39 | // 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. |
40 | // 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)); |
41 | TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) | (1 << WGM11) | (1 << WGM10)); |
53 | TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
42 | TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
54 | TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
43 | TCCR1B |= (1 << CS11) | (1 << ICES1) | (1 << ICNC1); |
55 | TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
44 | TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
Line 56... | Line 45... | ||
56 | 45 | ||
57 | // Timer/Counter1 Interrupt Mask Register |
46 | // Timer/Counter1 Interrupt Mask Register |
58 | // Enable Input Capture Interrupt (bit: ICIE1=1) |
47 | // Enable Input Capture Interrupt (bit: ICIE1=1) |
Line 86... | Line 75... | ||
86 | The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
75 | 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 |
76 | 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. |
77 | the syncronization gap. |
89 | */ |
78 | */ |
90 | ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
79 | ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
91 | int16_t signal = 0, tmp; |
80 | int16_t signal, tmp; |
92 | static int16_t index; |
81 | static int16_t index; |
93 | static uint16_t oldICR1 = 0; |
82 | static uint16_t oldICR1 = 0; |
Line 94... | Line 83... | ||
94 | 83 | ||
95 | // 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
84 | // 16bit Input Capture Register ICR1 contains the timer value TCNT1 |
Line 99... | Line 88... | ||
99 | // calculatiing the difference of the two uint16_t and converting the result to an int16_t |
88 | // 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. |
89 | // implicit handles a timer overflow 65535 -> 0 the right way. |
101 | signal = (uint16_t) ICR1 - oldICR1; |
90 | signal = (uint16_t) ICR1 - oldICR1; |
102 | oldICR1 = ICR1; |
91 | oldICR1 = ICR1; |
Line 103... | Line 92... | ||
103 | 92 | ||
104 | //sync gap? (3.52 ms < signal < 25.6 ms) |
93 | //sync gap? (3.5 ms < signal < 25.6 ms) |
- | 94 | if (signal > TIME(3.5)) { |
|
105 | if ((signal > 1100) && (signal < 8000)) { |
95 | // never happens... |
106 | index = 0; |
96 | index = 0; |
107 | } else { // within the PPM frame |
97 | } else { // within the PPM frame |
108 | if (index < MAX_CHANNELS) { // PPM24 supports 12 channels |
98 | 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 |
99 | // check for valid signal length (0.8 ms < signal < 2.2 ms) |
111 | if ((signal > 250) && (signal < 687)) { |
100 | if ((signal >= TIME(0.8)) && (signal < TIME(2.2))) { |
- | 101 | // shift signal to zero symmetric range -154 to 159 |
|
112 | // shift signal to zero symmetric range -154 to 159 |
102 | //signal -= 3750; // theoretical value |
113 | signal -= 475; // offset of 1.4912 ms ??? (469 * 3.2us = 1.5008 ms) |
103 | signal -= (TIME(1.5) + RC_TRIM); // best value with my Futaba in zero trim. |
114 | // check for stable signal |
104 | // check for stable signal |
115 | if (abs(signal - PPM_in[index]) < 6) { |
105 | if (abs(signal - PPM_in[index]) < TIME(0.05)) { |
116 | if (RCQuality < 200) |
106 | if (RCQuality < 200) |
117 | RCQuality += 10; |
107 | RCQuality += 10; |
118 | else |
108 | else |
119 | RCQuality = 200; |
109 | RCQuality = 200; |
Line 143... | Line 133... | ||
143 | } |
133 | } |
144 | } |
134 | } |
145 | } |
135 | } |
Line 146... | Line 136... | ||
146 | 136 | ||
147 | #define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
- | |
148 | #define COMMAND_THRESHOLD 85 |
137 | #define RCChannel(dimension) PPM_in[channelMap.channels[dimension]] |
149 | #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
138 | #define COMMAND_CHANNEL_VERTICAL CH_THROTTLE |
Line 150... | Line -... | ||
150 | #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
- | |
151 | - | ||
152 | #define RC_SCALING 4 |
139 | #define COMMAND_CHANNEL_HORIZONTAL CH_YAW |
153 | 140 | ||
154 | uint8_t getControlModeSwitch(void) { |
141 | uint8_t getControlModeSwitch(void) { |
155 | int16_t channel = RCChannel(CH_MODESWITCH) + POT_OFFSET; |
- | |
156 | uint8_t flightMode = channel < 256/3 ? FLIGHT_MODE_MANUAL : |
142 | int16_t channel = RCChannel(CH_MODESWITCH); |
157 | (channel > 256*2/3 ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE); |
143 | uint8_t flightMode = channel < -TIME(0.17) ? FLIGHT_MODE_MANUAL : (channel > TIME(0.17) ? FLIGHT_MODE_ANGLES : FLIGHT_MODE_RATE); |
Line 158... | Line 144... | ||
158 | return flightMode; |
144 | return flightMode; |
159 | } |
145 | } |
Line 172... | Line 158... | ||
172 | return lastRCCommand; |
158 | return lastRCCommand; |
173 | } |
159 | } |
Line 174... | Line 160... | ||
174 | 160 | ||
Line 175... | Line 161... | ||
175 | int16_t channel = RCChannel(CH_THROTTLE); |
161 | int16_t channel = RCChannel(CH_THROTTLE); |
176 | 162 | ||
- | 163 | if (channel <= -TIME(0.55)) { |
|
177 | if (channel <= -140) { // <= 900 us |
164 | lastRCCommand = COMMAND_GYROCAL; |
178 | lastRCCommand = COMMAND_GYROCAL; |
165 | debugOut.analog[17] = 1; |
- | 166 | } else { |
|
179 | } else { |
167 | lastRCCommand = COMMAND_NONE; |
180 | lastRCCommand = COMMAND_NONE; |
168 | debugOut.analog[17] = 0; |
181 | } |
169 | } |
Line 182... | Line 170... | ||
182 | return lastRCCommand; |
170 | return lastRCCommand; |
Line 196... | Line 184... | ||
196 | debugOut.analog[20] = RCChannel(CH_ELEVATOR); |
184 | debugOut.analog[20] = RCChannel(CH_ELEVATOR); |
197 | debugOut.analog[21] = RCChannel(CH_AILERONS); |
185 | debugOut.analog[21] = RCChannel(CH_AILERONS); |
198 | debugOut.analog[22] = RCChannel(CH_RUDDER); |
186 | debugOut.analog[22] = RCChannel(CH_RUDDER); |
199 | debugOut.analog[23] = RCChannel(CH_THROTTLE); |
187 | debugOut.analog[23] = RCChannel(CH_THROTTLE); |
Line 200... | Line 188... | ||
200 | 188 | ||
201 | PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) * RC_SCALING; |
189 | PRYT[CONTROL_ELEVATOR] = RCChannel(CH_ELEVATOR) / RC_SCALING; |
202 | PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) * RC_SCALING; |
190 | PRYT[CONTROL_AILERONS] = RCChannel(CH_AILERONS) / RC_SCALING; |
203 | PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) * RC_SCALING; |
191 | PRYT[CONTROL_RUDDER] = RCChannel(CH_RUDDER) / RC_SCALING; |
204 | PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) * RC_SCALING; |
192 | PRYT[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) / RC_SCALING; |
205 | } // if RCQuality is no good, we just do nothing. |
193 | } // if RCQuality is no good, we just do nothing. |
Line 206... | Line 194... | ||
206 | } |
194 | } |
207 | 195 | ||
208 | /* |
196 | /* |
209 | * Get other channel value |
197 | * Get other channel value |
210 | */ |
198 | */ |
211 | int16_t RC_getVariable(uint8_t varNum) { |
199 | int16_t RC_getVariable(uint8_t varNum) { |
212 | if (varNum < 4) |
200 | if (varNum < 4) |
213 | // 0th variable is 5th channel (1-based) etc. |
201 | // 0th variable is 5th channel (1-based) etc. |
214 | return RCChannel(varNum + CH_POTS) + POT_OFFSET; |
202 | return (RCChannel(varNum + CH_POTS) >> 2) + VARIABLE_OFFSET; |
215 | /* |
203 | /* |
216 | * Let's just say: |
204 | * Let's just say: |
217 | * The RC variable i is hardwired to channel i, i>=4 |
205 | * The RC variable i is hardwired to channel i, i>=4 |
218 | */ |
206 | */ |
Line 219... | Line 207... | ||
219 | return PPM_in[varNum] + POT_OFFSET; |
207 | return (PPM_in[varNum] >> 2) + VARIABLE_OFFSET; |
220 | } |
208 | } |
221 | 209 |