Rev 2062 | Rev 2069 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2062 | Rev 2067 | ||
---|---|---|---|
Line 32... | Line 32... | ||
32 | 32 | ||
33 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
33 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
Line 34... | Line 34... | ||
34 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
34 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
35 | 35 | ||
36 | /***************************************************** |
36 | /***************************************************** |
37 | * Initialize Timer 2 |
37 | * Initialize Timer 2 |
38 | *****************************************************/ |
38 | *****************************************************/ |
Line 39... | Line 39... | ||
39 | void timer2_init(void) { |
39 | void timer2_init(void) { |
40 | uint8_t sreg = SREG; |
40 | uint8_t sreg = SREG; |
Line 41... | Line 41... | ||
41 | 41 | ||
42 | // disable all interrupts before reconfiguration |
42 | // disable all interrupts before reconfiguration |
43 | cli(); |
43 | cli(); |
44 | 44 | ||
45 | // set PD7 as output of the PWM for pitch servo |
45 | // set PD7 as output of the PWM for pitch servo |
46 | DDRD |= (1 << DDD7); |
46 | DDRD |= (1 << DDD7); |
47 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
47 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
48 | 48 | ||
49 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
49 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
50 | HEF4017R_ON; // enable reset |
50 | HEF4017R_ON; // enable reset |
51 | 51 | ||
52 | // Timer/Counter 2 Control Register A |
52 | // Timer/Counter 2 Control Register A |
53 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
53 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
54 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
54 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
55 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
55 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
56 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
56 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
57 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
57 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
58 | 58 | ||
59 | // Timer/Counter 2 Control Register B |
59 | // Timer/Counter 2 Control Register B |
60 | 60 | ||
61 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
61 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
62 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
62 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
63 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
63 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
64 | 64 | ||
65 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
65 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
66 | TCCR2B |= CS2; |
66 | TCCR2B |= CS2; |
67 | 67 | ||
68 | // Initialize the Timer/Counter 2 Register |
68 | // Initialize the Timer/Counter 2 Register |
69 | TCNT2 = 0; |
69 | TCNT2 = 0; |
70 | 70 | ||
71 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
71 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
72 | OCR2A = 255; |
72 | OCR2A = 255; |
73 | 73 | ||
74 | // Timer/Counter 2 Interrupt Mask Register |
74 | // Timer/Counter 2 Interrupt Mask Register |
75 | // Enable timer output compare match A Interrupt only |
75 | // Enable timer output compare match A Interrupt only |
76 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
76 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
77 | TIMSK2 |= (1 << OCIE2A); |
77 | TIMSK2 |= (1 << OCIE2A); |
78 | 78 | ||
79 | for (uint8_t axis=0; axis<2; axis++) |
79 | for (uint8_t axis=0; axis<2; axis++) |
Line 80... | Line 80... | ||
80 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
80 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
81 | 81 | ||
82 | SREG = sreg; |
82 | SREG = sreg; |
83 | } |
83 | } |
84 | 84 | ||
85 | /* |
85 | /* |
86 | void servo_On(void) { |
86 | void servo_On(void) { |
87 | servoActive = 1; |
87 | servoActive = 1; |
88 | } |
88 | } |
Line 89... | Line 89... | ||
89 | void servo_Off(void) { |
89 | void servo_Off(void) { |
90 | servoActive = 0; |
90 | servoActive = 0; |
91 | HEF4017R_ON; // enable reset |
91 | HEF4017R_ON; // enable reset |
Line 92... | Line 92... | ||
92 | } |
92 | } |
93 | */ |
93 | */ |
94 | 94 | ||
Line 109... | Line 109... | ||
109 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
109 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
110 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
110 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
111 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
111 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
112 | value /= 256L; |
112 | value /= 256L; |
113 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
113 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
114 | return -value; |
114 | return -value; |
115 | return value; |
115 | return value; |
116 | } |
116 | } |
Line 117... | Line 117... | ||
117 | 117 | ||
118 | // With constant-speed limitation. |
118 | // With constant-speed limitation. |
Line 140... | Line 140... | ||
140 | 140 | ||
141 | uint16_t servoValue(uint8_t axis) { |
141 | uint16_t servoValue(uint8_t axis) { |
142 | int16_t value; |
142 | int16_t value; |
143 | if (axis<2) value = featuredServoValue(axis); |
143 | if (axis<2) value = featuredServoValue(axis); |
144 | else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
144 | else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
145 | // Shift out of the [0..255*SCALE_FACTOR] space |
145 | // Shift out of the [0..255*SCALE_FACTOR] space |
146 | value -= (128 * SCALE_FACTOR); |
146 | value -= (128 * SCALE_FACTOR); |
147 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
147 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
148 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
148 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
149 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
149 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
150 | return value + NEUTRAL_PULSELENGTH; |
150 | return value + NEUTRAL_PULSELENGTH; |
Line 151... | Line 151... | ||
151 | } |
151 | } |
152 | 152 | ||
153 | void calculateServoValues(void) { |
153 | void calculateServoValues(void) { |
154 | if (!recalculateServoTimes) return; |
154 | if (!recalculateServoTimes) return; |
155 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
155 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
156 | servoValues[axis] = servoValue(axis); |
156 | servoValues[axis] = servoValue(axis); |
157 | } |
157 | } |
Line 158... | Line 158... | ||
158 | recalculateServoTimes = 0; |
158 | recalculateServoTimes = 0; |
159 | } |
159 | } |
160 | 160 | ||
161 | ISR(TIMER2_COMPA_vect) { |
161 | ISR(TIMER2_COMPA_vect) { |
162 | static uint16_t remainingPulseTime; |
162 | static uint16_t remainingPulseTime; |
163 | static uint8_t servoIndex = 0; |
163 | static uint8_t servoIndex = 0; |
164 | static uint16_t sumOfPulseTimes = 0; |
164 | static uint16_t sumOfPulseTimes = 0; |
165 | 165 | ||
166 | if (!remainingPulseTime) { |
166 | if (!remainingPulseTime) { |
167 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
167 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
Line 178... | Line 178... | ||
178 | } |
178 | } |
179 | } |
179 | } |
Line 180... | Line 180... | ||
180 | 180 | ||
181 | // Schedule the next OCR2A event. The counter is already reset at this time. |
181 | // Schedule the next OCR2A event. The counter is already reset at this time. |
182 | if (remainingPulseTime > 256+128) { |
182 | if (remainingPulseTime > 256+128) { |
183 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
183 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
184 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
184 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
185 | OCR2A = 255; |
185 | OCR2A = 255; |
186 | TCCR2A &= ~(1<<COM2A0); |
186 | TCCR2A &= ~(1<<COM2A0); |
187 | remainingPulseTime-=256; |
187 | remainingPulseTime-=256; |
188 | } else if (remainingPulseTime > 256) { |
188 | } else if (remainingPulseTime > 256) { |
189 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
189 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
190 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
190 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
191 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
191 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
192 | remainingPulseTime-=128; |
192 | remainingPulseTime-=128; |
193 | OCR2A=127; |
193 | OCR2A=127; |