Rev 2164 | Details | Compare with Previous | Last modification | View Log | RSS feed
Rev | Author | Line No. | Line |
---|---|---|---|
1612 | dongfang | 1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
||
2189 | - | 3 | #include "timer2.h" |
1612 | dongfang | 4 | |
2189 | - | 5 | #if (SERVO_RESOLUTION == COARSE) |
1980 | - | 6 | #define CS2 ((1<<CS21)|(1<<CS20)) |
7 | #else |
||
2062 | - | 8 | #define CS2 (1<<CS21) |
1980 | - | 9 | #endif |
10 | |||
2189 | - | 11 | #define FRAMELEN (PULSELENGTH_2200 * staticParams.servoCount + 128) |
1980 | - | 12 | |
2189 | - | 13 | // volatile uint8_t servoActive = 0; |
14 | // volatile uint8_t recalculateServoTimes = 0; |
||
15 | volatile uint16_t pwmChannels[MAX_PWMCHANNELS]; |
||
1980 | - | 16 | |
1612 | dongfang | 17 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
18 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
||
19 | |||
20 | /***************************************************** |
||
2067 | - | 21 | * Initialize Timer 2 |
1612 | dongfang | 22 | *****************************************************/ |
23 | void timer2_init(void) { |
||
2067 | - | 24 | uint8_t sreg = SREG; |
1612 | dongfang | 25 | |
2067 | - | 26 | // disable all interrupts before reconfiguration |
27 | cli(); |
||
1612 | dongfang | 28 | |
2067 | - | 29 | // set PD7 as output of the PWM for pitch servo |
30 | DDRD |= (1 << DDD7); |
||
31 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
||
1612 | dongfang | 32 | |
2067 | - | 33 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
34 | HEF4017R_ON; // enable reset |
||
1612 | dongfang | 35 | |
2067 | - | 36 | // Timer/Counter 2 Control Register A |
37 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
||
38 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
||
39 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
||
40 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
||
41 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
||
1612 | dongfang | 42 | |
2067 | - | 43 | // Timer/Counter 2 Control Register B |
1612 | dongfang | 44 | |
2067 | - | 45 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
46 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
||
47 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
||
48 | |||
49 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
||
50 | TCCR2B |= CS2; |
||
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 | |||
58 | // Timer/Counter 2 Interrupt Mask Register |
||
59 | // Enable timer output compare match A Interrupt only |
||
60 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
||
61 | TIMSK2 |= (1 << OCIE2A); |
||
62 | |||
63 | SREG = sreg; |
||
1612 | dongfang | 64 | } |
65 | |||
1980 | - | 66 | /* |
67 | void servo_On(void) { |
||
2067 | - | 68 | servoActive = 1; |
1612 | dongfang | 69 | } |
1980 | - | 70 | void servo_Off(void) { |
2067 | - | 71 | servoActive = 0; |
72 | HEF4017R_ON; // enable reset |
||
1612 | dongfang | 73 | } |
1980 | - | 74 | */ |
1612 | dongfang | 75 | |
76 | /***************************************************** |
||
2067 | - | 77 | * Control Servo Position |
1612 | dongfang | 78 | *****************************************************/ |
1821 | - | 79 | |
2189 | - | 80 | /* |
1980 | - | 81 | void calculateServoValues(void) { |
82 | if (!recalculateServoTimes) return; |
||
83 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
||
2067 | - | 84 | servoValues[axis] = servoValue(axis); |
85 | } |
||
1980 | - | 86 | recalculateServoTimes = 0; |
87 | } |
||
2189 | - | 88 | */ |
1821 | - | 89 | |
1980 | - | 90 | ISR(TIMER2_COMPA_vect) { |
91 | static uint16_t remainingPulseTime; |
||
92 | static uint8_t servoIndex = 0; |
||
93 | static uint16_t sumOfPulseTimes = 0; |
||
2067 | - | 94 | |
1980 | - | 95 | if (!remainingPulseTime) { |
96 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
||
97 | if (servoIndex < staticParams.servoCount) { |
||
98 | // There are more signals to output. |
||
2189 | - | 99 | sumOfPulseTimes += (remainingPulseTime = pwmChannels[servoIndex]); //pwmChannels[servoMap[servoIndex]]); |
1980 | - | 100 | servoIndex++; |
101 | } else { |
||
102 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
||
103 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
||
104 | sumOfPulseTimes = servoIndex = 0; |
||
105 | HEF4017R_ON; |
||
106 | } |
||
107 | } |
||
2189 | - | 108 | |
1980 | - | 109 | // Schedule the next OCR2A event. The counter is already reset at this time. |
110 | if (remainingPulseTime > 256+128) { |
||
2067 | - | 111 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
1980 | - | 112 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
113 | OCR2A = 255; |
||
114 | TCCR2A &= ~(1<<COM2A0); |
||
2189 | - | 115 | remainingPulseTime -= 256; |
1980 | - | 116 | } else if (remainingPulseTime > 256) { |
2067 | - | 117 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
1980 | - | 118 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
119 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
||
120 | OCR2A=127; |
||
2189 | - | 121 | remainingPulseTime -= 128; |
1980 | - | 122 | } else { |
123 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
||
124 | TCCR2A |= (1<<COM2A0); |
||
125 | OCR2A = remainingPulseTime-1; |
||
2189 | - | 126 | remainingPulseTime = 0; |
1980 | - | 127 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
128 | } |
||
1612 | dongfang | 129 | } |