Rev 2099 | Go to most recent revision | Details | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1910 | - | 1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
||
3 | #include "timer2.h" |
||
4 | #include "eeprom.h" |
||
5 | #include "uart0.h" |
||
6 | #include "rc.h" |
||
7 | #include "attitude.h" |
||
8 | |||
9 | #define HEF4017_RESET_HIGH PORTC |= (1<<PORTC6) |
||
10 | #define HEF4017_RESET_LOW PORTC &= ~(1<<PORTC6) |
||
11 | |||
12 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
||
13 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
||
14 | |||
15 | OutputData_t outputs[MAX_OUTPUTS]; |
||
16 | |||
17 | /***************************************************** |
||
18 | * Initialize Timer 2 |
||
19 | *****************************************************/ |
||
20 | void timer2_init(void) { |
||
21 | uint8_t sreg = SREG; |
||
22 | |||
23 | // disable all interrupts before reconfiguration |
||
24 | cli(); |
||
25 | |||
26 | // set PD7 as output of the PWM for pitch servo |
||
27 | DDRD |= (1 << DDD7); |
||
28 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
||
29 | |||
30 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
||
31 | //PORTC &= ~(1<<PORTC6); // set PC6 to low |
||
32 | HEF4017_RESET_HIGH; // enable reset |
||
33 | |||
34 | // Timer/Counter 2 Control Register A |
||
35 | |||
36 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
||
37 | // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
||
38 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
||
39 | TCCR2A &= ~((1 << COM2A1) | (1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0)); |
||
40 | TCCR2A |= (1 << WGM21); |
||
41 | |||
42 | // Timer/Counter 2 Control Register B |
||
43 | |||
44 | // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz |
||
45 | // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us |
||
46 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms |
||
47 | |||
48 | // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1) |
||
49 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS22)); |
||
50 | TCCR2B |= (1 << CS21) | (1 << CS20); |
||
51 | |||
52 | // Initialize the Timer/Counter 2 Register |
||
53 | TCNT2 = 0; |
||
54 | |||
55 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
||
56 | OCR2A = 255; |
||
57 | TCCR2A |= (1 << COM2A1); // set or clear at compare match depends on value of COM2A0 |
||
58 | |||
59 | // Timer/Counter 2 Interrupt Mask Register |
||
60 | // Enable timer output compare match A Interrupt only |
||
61 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
||
62 | TIMSK2 |= (1 << OCIE2A); |
||
63 | |||
64 | SREG = sreg; |
||
65 | } |
||
66 | |||
67 | /***************************************************** |
||
68 | * Control Servo Position |
||
69 | *****************************************************/ |
||
70 | const uint8_t SERVO_REMAPPING[MAX_OUTPUTS] = {0,0,1,2,3,4,5,6}; |
||
71 | #define NEUTRAL_PULSELENGTH 937 |
||
72 | #define SERVOLIMIT 500 |
||
73 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.ServoRefresh + 128) |
||
74 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
||
75 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
||
76 | |||
77 | ISR(TIMER2_COMPA_vect) { |
||
78 | static uint16_t remainingPulseTime = 0; |
||
79 | static uint8_t servoIndex = 0; |
||
80 | static uint16_t sumOfPulseTimes = 0; |
||
81 | |||
82 | if (!remainingPulseTime) { |
||
83 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
||
84 | if (servoIndex < staticParams.ServoRefresh) { |
||
85 | // There are more signals to output. |
||
86 | remainingPulseTime = NEUTRAL_PULSELENGTH + outputs[SERVO_REMAPPING[servoIndex]].SetPoint; |
||
87 | if (remainingPulseTime < MIN_PULSELENGTH) remainingPulseTime = MIN_PULSELENGTH; |
||
88 | else if (remainingPulseTime > MAX_PULSELENGTH) remainingPulseTime = MAX_PULSELENGTH; |
||
89 | sumOfPulseTimes += remainingPulseTime; |
||
90 | servoIndex++; |
||
91 | } else { |
||
92 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
||
93 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
||
94 | sumOfPulseTimes = servoIndex = 0; |
||
95 | HEF4017_RESET_HIGH; |
||
96 | } |
||
97 | } |
||
98 | |||
99 | // Schedule the next OCR2A event. The counter is already reset at this time. |
||
100 | uint8_t delta; |
||
101 | if (remainingPulseTime > 255+128) { |
||
102 | delta = 255; |
||
103 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
||
104 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
||
105 | TCCR2A &= ~(1<<COM2A0); |
||
106 | } else if (remainingPulseTime > 255) { |
||
107 | // Remaining pulse lengths in the range [256..256+something small] might cause trouble if handled the standard |
||
108 | // way, which is in chunks of 255. The remainder would be very small, possibly causing an interrupt on interrupt |
||
109 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
||
110 | delta = 128; |
||
111 | } else { |
||
112 | delta = remainingPulseTime; |
||
113 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
||
114 | TCCR2A |= (1<<COM2A0); |
||
115 | HEF4017_RESET_LOW; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
||
116 | } |
||
117 | OCR2A = delta; |
||
118 | remainingPulseTime -= delta; |
||
119 | } |