Rev 1796 | Rev 1868 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1796 | Rev 1821 | ||
---|---|---|---|
Line 103... | Line 103... | ||
103 | // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
103 | // Set clock source to SYSCLK/64 (bit: CS12=0, CS11=1, CS10=1) |
104 | // Enable input capture noise cancler (bit: ICNC1=1) |
104 | // Enable input capture noise cancler (bit: ICNC1=1) |
105 | // Trigger on positive edge of the input capture pin (bit: ICES1=1), |
105 | // Trigger on positive edge of the input capture pin (bit: ICES1=1), |
106 | // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
106 | // Therefore the counter incremets at a clock of 20 MHz/64 = 312.5 kHz or 3.2µs |
107 | // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
107 | // The longest period is 0xFFFF / 312.5 kHz = 0.209712 s. |
108 | TCCR1A &= ~((1<<COM1A1)|(1<<COM1A0)|(1<<COM1B1)|(1<<COM1B0)|(1<<WGM11)|(1<<WGM10)); |
108 | TCCR1A &= ~((1 << COM1A1) | (1 << COM1A0) | (1 << COM1B1) | (1 << COM1B0) |
- | 109 | | (1 << WGM11) | (1 << WGM10)); |
|
109 | TCCR1B &= ~((1<<WGM13)|(1<<WGM12)|(1<<CS12)); |
110 | TCCR1B &= ~((1 << WGM13) | (1 << WGM12) | (1 << CS12)); |
110 | TCCR1B |= (1<<CS11)|(1<<CS10)|(1<<ICES1)|(1<<ICNC1); |
111 | TCCR1B |= (1 << CS11) | (1 << CS10) | (1 << ICES1) | (1 << ICNC1); |
111 | TCCR1C &= ~((1<<FOC1A)|(1<<FOC1B)); |
112 | TCCR1C &= ~((1 << FOC1A) | (1 << FOC1B)); |
Line 112... | Line 113... | ||
112 | 113 | ||
Line 142... | Line 143... | ||
142 | The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
143 | The minimum duration of all channels at minimum value is 8 * 1 ms = 8 ms. |
143 | The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
144 | The maximum duration of all channels at maximum value is 8 * 2 ms = 16 ms. |
144 | The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
145 | The remaining time of (22.5 - 8 ms) ms = 14.5 ms to (22.5 - 16 ms) ms = 6.5 ms is |
145 | the syncronization gap. |
146 | the syncronization gap. |
146 | */ |
147 | */ |
- | 148 | ISR(TIMER1_CAPT_vect) |
|
147 | ISR(TIMER1_CAPT_vect) { // typical rate of 1 ms to 2 ms |
149 | { // typical rate of 1 ms to 2 ms |
148 | int16_t signal = 0, tmp; |
150 | int16_t signal = 0, tmp; |
149 | static int16_t index; |
151 | static int16_t index; |
150 | static uint16_t oldICR1 = 0; |
152 | static uint16_t oldICR1 = 0; |
Line 151... | Line 153... | ||
151 | 153 | ||
Line 174... | Line 176... | ||
174 | if((signal > 250) && (signal < 687)) { |
176 | if ((signal > 250) && (signal < 687)) { |
175 | // shift signal to zero symmetric range -154 to 159 |
177 | // shift signal to zero symmetric range -154 to 159 |
176 | signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
178 | signal -= 470; // offset of 1.4912 ms ??? (469 * 3.2µs = 1.5008 ms) |
177 | // check for stable signal |
179 | // check for stable signal |
178 | if(abs(signal - PPM_in[index]) < 6) { |
180 | if (abs(signal - PPM_in[index]) < 6) { |
179 | if(RC_Quality < 200) RC_Quality +=10; |
181 | if (RC_Quality < 200) |
- | 182 | RC_Quality += 10; |
|
- | 183 | else |
|
180 | else RC_Quality = 200; |
184 | RC_Quality = 200; |
181 | } |
185 | } |
182 | // If signal is the same as before +/- 1, just keep it there. |
186 | // If signal is the same as before +/- 1, just keep it there. |
183 | if (signal>=PPM_in[index]-1 && signal<=PPM_in[index]+1) { |
187 | if (signal >= PPM_in[index] - 1 && signal <= PPM_in[index] + 1) { |
184 | // In addition, if the signal is very close to 0, just set it to 0. |
188 | // In addition, if the signal is very close to 0, just set it to 0. |
185 | if (signal >=-1 && signal <= 1) { |
189 | if (signal >= -1 && signal <= 1) { |
186 | tmp = 0; |
190 | tmp = 0; |
187 | } else { |
191 | } else { |
188 | tmp = PPM_in[index]; |
192 | tmp = PPM_in[index]; |
189 | } |
193 | } |
190 | } |
- | |
191 | else |
194 | } else |
192 | tmp = signal; |
195 | tmp = signal; |
193 | // calculate signal difference on good signal level |
196 | // calculate signal difference on good signal level |
194 | if(RC_Quality >= 195) |
197 | if (RC_Quality >= 195) |
195 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
198 | PPM_diff[index] = ((tmp - PPM_in[index]) / 3) * 3; // cut off lower 3 bit for nois reduction |
- | 199 | else |
|
196 | else PPM_diff[index] = 0; |
200 | PPM_diff[index] = 0; |
197 | PPM_in[index] = tmp; // update channel value |
201 | PPM_in[index] = tmp; // update channel value |
198 | } |
202 | } |
199 | index++; // next channel |
203 | index++; // next channel |
200 | // demux sum signal for channels 5 to 7 to J3, J4, J5 |
204 | // demux sum signal for channels 5 to 7 to J3, J4, J5 |
201 | // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
205 | // TODO: General configurability of this R/C channel forwarding. Or remove it completely - the |
Line 243... | Line 247... | ||
243 | void RC_update() { |
247 | void RC_update() { |
244 | int16_t tmp1, tmp2; |
248 | int16_t tmp1, tmp2; |
245 | if(RC_Quality) { |
249 | if (RC_Quality) { |
246 | RC_Quality--; |
250 | RC_Quality--; |
247 | if (NewPpmData-- == 0) { |
251 | if (NewPpmData-- == 0) { |
248 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP + RCDiff(CH_PITCH) * staticParams.StickD; |
252 | RC_PRTY[CONTROL_PITCH] = RCChannel(CH_PITCH) * staticParams.StickP |
- | 253 | + RCDiff(CH_PITCH) * staticParams.StickD; |
|
249 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP + RCDiff(CH_ROLL) * staticParams.StickD; |
254 | RC_PRTY[CONTROL_ROLL] = RCChannel(CH_ROLL) * staticParams.StickP |
- | 255 | + RCDiff(CH_ROLL) * staticParams.StickD; |
|
250 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) * dynamicParams.UserParams[3] + 120; |
256 | RC_PRTY[CONTROL_THROTTLE] = RCChannel(CH_THROTTLE) + RCDiff(CH_THROTTLE) |
- | 257 | * dynamicParams.UserParams[3] + 120; |
|
- | 258 | if (RC_PRTY[CONTROL_THROTTLE] < 0) |
|
251 | if (RC_PRTY[CONTROL_THROTTLE] < 0) RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
259 | RC_PRTY[CONTROL_THROTTLE] = 0; // Throttle is non negative. |
252 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
260 | tmp1 = -RCChannel(CH_YAW) - RCDiff(CH_YAW); |
253 | // exponential stick sensitivity in yawing rate |
261 | // exponential stick sensitivity in yawing rate |
254 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t)tmp1 * abs(tmp1)) / 512L; // expo y = ax + bx^2 |
262 | tmp2 = (int32_t) staticParams.StickYawP * ((int32_t) tmp1 * abs(tmp1)) |
- | 263 | / 512L; // expo y = ax + bx^2 |
|
255 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |
264 | tmp2 += (staticParams.StickYawP * tmp1) / 4; |
256 | RC_PRTY[CONTROL_YAW] = tmp2; |
265 | RC_PRTY[CONTROL_YAW] = tmp2; |
257 | } |
266 | } |
258 | uint8_t command = RC_getStickCommand(); |
267 | uint8_t command = RC_getStickCommand(); |
259 | if (lastRCCommand == command) { |
268 | if (lastRCCommand == command) { |
260 | // Keep timer from overrunning. |
269 | // Keep timer from overrunning. |
261 | if (commandTimer < COMMAND_TIMER) commandTimer++; |
270 | if (commandTimer < COMMAND_TIMER) |
- | 271 | commandTimer++; |
|
262 | } else { |
272 | } else { |
263 | // There was a change. |
273 | // There was a change. |
264 | lastRCCommand = command; |
274 | lastRCCommand = command; |
265 | commandTimer = 0; |
275 | commandTimer = 0; |
266 | } |
276 | } |
267 | } else { // Bad signal |
277 | } else { // Bad signal |
268 | RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] = RC_PRTY[CONTROL_YAW] = 0; |
278 | RC_PRTY[CONTROL_PITCH] = RC_PRTY[CONTROL_ROLL] = RC_PRTY[CONTROL_THROTTLE] |
- | 279 | = RC_PRTY[CONTROL_YAW] = 0; |
|
269 | } |
280 | } |
270 | } |
281 | } |
Line 271... | Line 282... | ||
271 | 282 | ||
272 | /* |
283 | /* |
Line 381... | Line 392... | ||
381 | } |
392 | } |
Line 382... | Line 393... | ||
382 | 393 | ||
383 | uint8_t RC_getLooping(uint8_t looping) { |
394 | uint8_t RC_getLooping(uint8_t looping) { |
Line 384... | Line 395... | ||
384 | // static uint8_t looping = 0; |
395 | // static uint8_t looping = 0; |
- | 396 | ||
385 | 397 | if (RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig |
|
- | 398 | & CFG_LOOP_LEFT) { |
|
386 | if(RCChannel(CH_ROLL) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_LEFT) { |
399 | looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
387 | looping |= (LOOPING_ROLL_AXIS | LOOPING_LEFT); |
400 | } else if ((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) |
388 | } else if((looping & LOOPING_LEFT) && RCChannel(CH_ROLL) < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
401 | < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
Line 389... | Line 402... | ||
389 | looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
402 | looping &= (~(LOOPING_ROLL_AXIS | LOOPING_LEFT)); |
- | 403 | } |
|
390 | } |
404 | |
- | 405 | if (RCChannel(CH_ROLL) < -staticParams.LoopThreshold |
|
391 | 406 | && staticParams.BitConfig & CFG_LOOP_RIGHT) { |
|
392 | if(RCChannel(CH_ROLL) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_RIGHT) { |
407 | looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
393 | looping |= (LOOPING_ROLL_AXIS | LOOPING_RIGHT); |
408 | } else if ((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) |
Line 394... | Line 409... | ||
394 | } else if((looping & LOOPING_RIGHT) && RCChannel(CH_ROLL) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
409 | > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
- | 410 | looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
|
395 | looping &= (~(LOOPING_ROLL_AXIS | LOOPING_RIGHT)); |
411 | } |
- | 412 | ||
396 | } |
413 | if (RCChannel(CH_PITCH) > staticParams.LoopThreshold |
397 | 414 | && staticParams.BitConfig & CFG_LOOP_UP) { |
|
398 | if(RCChannel(CH_PITCH) > staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_UP) { |
415 | looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
Line 399... | Line 416... | ||
399 | looping |= (LOOPING_PITCH_AXIS | LOOPING_UP); |
416 | } else if ((looping & LOOPING_UP) && RCChannel(CH_PITCH) |
- | 417 | < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
|
400 | } else if((looping & LOOPING_UP) && RCChannel(CH_PITCH) < staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
418 | looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
- | 419 | } |
|
401 | looping &= (~(LOOPING_PITCH_AXIS | LOOPING_UP)); |
420 | |
402 | } |
421 | if (RCChannel(CH_PITCH) < -staticParams.LoopThreshold |
403 | 422 | && staticParams.BitConfig & CFG_LOOP_DOWN) { |
|
Line 404... | Line 423... | ||
404 | if(RCChannel(CH_PITCH) < -staticParams.LoopThreshold && staticParams.BitConfig & CFG_LOOP_DOWN) { |
423 | looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
405 | looping |= (LOOPING_PITCH_AXIS | LOOPING_DOWN); |
424 | } else if ((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) |
Line 406... | Line 425... | ||
406 | } else if((looping & LOOPING_DOWN) && RCChannel(CH_PITCH) > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
425 | > -staticParams.LoopThreshold - staticParams.LoopHysteresis) { |
407 | looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
426 | looping &= (~(LOOPING_PITCH_AXIS | LOOPING_DOWN)); |
408 | } |
427 | } |
409 | 428 | ||
- | 429 | return looping; |
|
410 | return looping; |
430 | } |
411 | } |
431 | |
412 | 432 | uint8_t RC_testCompassCalState(void) { |
|
413 | uint8_t RC_testCompassCalState(void) { |
433 | static uint8_t stick = 1; |
414 | static uint8_t stick = 1; |
434 | // if pitch is centered or top set stick to zero |