Rev 2164 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2164 | Rev 2189 | ||
---|---|---|---|
1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
2 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
3 | #include "eeprom.h" |
- | |
4 | #include "rc.h" |
3 | #include "timer2.h" |
5 | #include "attitude.h" |
- | |
6 | - | ||
7 | #define COARSERESOLUTION 1 |
- | |
8 | - | ||
9 | #ifdef COARSERESOLUTION |
- | |
10 | #define NEUTRAL_PULSELENGTH 938 |
- | |
11 | #define STABILIZATION_LOG_DIVIDER 6 |
- | |
12 | #define SERVOLIMIT 500 |
- | |
13 | #define SCALE_FACTOR 4 |
- | |
14 | #define CS2 ((1<<CS21)|(1<<CS20)) |
- | |
- | 4 | ||
- | 5 | #if (SERVO_RESOLUTION == COARSE) |
|
15 | 6 | #define CS2 ((1<<CS21)|(1<<CS20)) |
|
16 | #else |
- | |
17 | - | ||
18 | #define NEUTRAL_PULSELENGTH 3750 |
- | |
19 | #define STABILIZATION_LOG_DIVIDER 4 |
- | |
20 | #define SERVOLIMIT 2000 |
- | |
21 | #define SCALE_FACTOR 16 |
7 | #else |
22 | #define CS2 (1<<CS21) |
8 | #define CS2 (1<<CS21) |
23 | #endif |
9 | #endif |
24 | - | ||
25 | #define MAX_SERVOS 8 |
10 | |
26 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
- | |
27 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
- | |
28 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
11 | #define FRAMELEN (PULSELENGTH_2200 * staticParams.servoCount + 128) |
29 | 12 | ||
30 | //volatile uint8_t servoActive = 0; |
13 | // volatile uint8_t servoActive = 0; |
31 | volatile uint8_t recalculateServoTimes = 0; |
14 | // volatile uint8_t recalculateServoTimes = 0; |
32 | volatile uint16_t servoValues[MAX_SERVOS]; |
- | |
33 | volatile uint16_t previousManualValues[2]; |
15 | volatile uint16_t pwmChannels[MAX_PWMCHANNELS]; |
34 | 16 | ||
35 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
17 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
36 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
18 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
37 | 19 | ||
38 | /***************************************************** |
20 | /***************************************************** |
39 | * Initialize Timer 2 |
21 | * Initialize Timer 2 |
40 | *****************************************************/ |
22 | *****************************************************/ |
41 | void timer2_init(void) { |
23 | void timer2_init(void) { |
42 | uint8_t sreg = SREG; |
24 | uint8_t sreg = SREG; |
43 | 25 | ||
44 | // disable all interrupts before reconfiguration |
26 | // disable all interrupts before reconfiguration |
45 | cli(); |
27 | cli(); |
46 | 28 | ||
47 | // set PD7 as output of the PWM for pitch servo |
29 | // set PD7 as output of the PWM for pitch servo |
48 | DDRD |= (1 << DDD7); |
30 | DDRD |= (1 << DDD7); |
49 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
31 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
50 | 32 | ||
51 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
33 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
52 | HEF4017R_ON; // enable reset |
34 | HEF4017R_ON; // enable reset |
53 | 35 | ||
54 | // Timer/Counter 2 Control Register A |
36 | // Timer/Counter 2 Control Register A |
55 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
37 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
56 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
38 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
57 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
39 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
58 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
40 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
59 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
41 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
60 | 42 | ||
61 | // Timer/Counter 2 Control Register B |
43 | // Timer/Counter 2 Control Register B |
62 | 44 | ||
63 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
45 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
64 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
46 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 kHz or 0.4 us |
65 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
47 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
66 | 48 | ||
67 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
49 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
68 | TCCR2B |= CS2; |
50 | TCCR2B |= CS2; |
69 | 51 | ||
70 | // Initialize the Timer/Counter 2 Register |
52 | // Initialize the Timer/Counter 2 Register |
71 | TCNT2 = 0; |
53 | TCNT2 = 0; |
72 | 54 | ||
73 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
55 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
74 | OCR2A = 255; |
56 | OCR2A = 255; |
75 | 57 | ||
76 | // Timer/Counter 2 Interrupt Mask Register |
58 | // Timer/Counter 2 Interrupt Mask Register |
77 | // Enable timer output compare match A Interrupt only |
59 | // Enable timer output compare match A Interrupt only |
78 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
60 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
79 | TIMSK2 |= (1 << OCIE2A); |
61 | TIMSK2 |= (1 << OCIE2A); |
80 | - | ||
81 | for (uint8_t axis=0; axis<2; axis++) |
- | |
82 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
- | |
83 | 62 | ||
84 | SREG = sreg; |
63 | SREG = sreg; |
85 | } |
64 | } |
86 | 65 | ||
87 | /* |
66 | /* |
88 | void servo_On(void) { |
67 | void servo_On(void) { |
89 | servoActive = 1; |
68 | servoActive = 1; |
90 | } |
69 | } |
91 | void servo_Off(void) { |
70 | void servo_Off(void) { |
92 | servoActive = 0; |
71 | servoActive = 0; |
93 | HEF4017R_ON; // enable reset |
72 | HEF4017R_ON; // enable reset |
94 | } |
73 | } |
95 | */ |
74 | */ |
96 | 75 | ||
97 | /***************************************************** |
76 | /***************************************************** |
98 | * Control Servo Position |
77 | * Control Servo Position |
99 | *****************************************************/ |
78 | *****************************************************/ |
100 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
- | |
101 | int32_t value = attitude[axis] >> STABILIZATION_LOG_DIVIDER; // between -500000 to 500000 extreme limits. Just about |
- | |
102 | // With full blast on stabilization gain (255) we want to convert a delta of, say, 125000 to 2000. |
- | |
103 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
- | |
104 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
- | |
105 | value = value >> 8; |
- | |
106 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
- | |
107 | return -value; |
- | |
108 | return value; |
- | |
109 | } |
- | |
110 | - | ||
111 | // With constant-speed limitation. |
- | |
112 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
- | |
113 | int16_t diff = manualValue - previousManualValues[axis]; |
- | |
114 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
- | |
115 | if (diff > maxSpeed) diff = maxSpeed; |
- | |
116 | else if (diff < -maxSpeed) diff = -maxSpeed; |
- | |
117 | manualValue = previousManualValues[axis] + diff; |
- | |
118 | previousManualValues[axis] = manualValue; |
- | |
119 | return manualValue; |
- | |
120 | } |
- | |
121 | - | ||
122 | // add stabilization and manual, apply soft position limits. |
- | |
123 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
- | |
124 | int16_t featuredServoValue(uint8_t axis) { |
- | |
125 | int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
- | |
126 | value += calculateStabilizedServoAxis(axis); |
- | |
127 | int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
- | |
128 | if (value < limit) value = limit; |
- | |
129 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
- | |
130 | if (value > limit) value = limit; |
- | |
131 | return value; |
- | |
132 | } |
- | |
133 | - | ||
134 | uint16_t servoValue(uint8_t axis) { |
- | |
135 | int16_t value; |
- | |
136 | if (axis<2) value = featuredServoValue(axis); |
- | |
137 | else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
- | |
138 | // Shift out of the [0..255*SCALE_FACTOR] space |
- | |
139 | value -= (128 * SCALE_FACTOR); |
- | |
140 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
- | |
141 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
- | |
142 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
- | |
143 | return value + NEUTRAL_PULSELENGTH; |
- | |
144 | } |
- | |
- | 79 | ||
145 | 80 | /* |
|
146 | void calculateServoValues(void) { |
81 | void calculateServoValues(void) { |
147 | if (!recalculateServoTimes) return; |
82 | if (!recalculateServoTimes) return; |
148 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
83 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
149 | servoValues[axis] = servoValue(axis); |
84 | servoValues[axis] = servoValue(axis); |
150 | } |
85 | } |
151 | recalculateServoTimes = 0; |
86 | recalculateServoTimes = 0; |
152 | } |
87 | } |
153 | 88 | */ |
|
154 | uint8_t servoMap[] = {2,3,0,1,4,5,6,7}; |
- | |
155 | 89 | ||
156 | ISR(TIMER2_COMPA_vect) { |
90 | ISR(TIMER2_COMPA_vect) { |
157 | static uint16_t remainingPulseTime; |
91 | static uint16_t remainingPulseTime; |
158 | static uint8_t servoIndex = 0; |
92 | static uint8_t servoIndex = 0; |
159 | static uint16_t sumOfPulseTimes = 0; |
93 | static uint16_t sumOfPulseTimes = 0; |
160 | 94 | ||
161 | if (!remainingPulseTime) { |
95 | if (!remainingPulseTime) { |
162 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
96 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
163 | if (servoIndex < staticParams.servoCount) { |
97 | if (servoIndex < staticParams.servoCount) { |
164 | // There are more signals to output. |
98 | // There are more signals to output. |
165 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoMap[servoIndex]]); |
99 | sumOfPulseTimes += (remainingPulseTime = pwmChannels[servoIndex]); //pwmChannels[servoMap[servoIndex]]); |
166 | servoIndex++; |
100 | servoIndex++; |
167 | } else { |
101 | } else { |
168 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
102 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
169 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
103 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
170 | sumOfPulseTimes = servoIndex = 0; |
104 | sumOfPulseTimes = servoIndex = 0; |
171 | recalculateServoTimes = 1; |
- | |
172 | HEF4017R_ON; |
105 | HEF4017R_ON; |
173 | } |
106 | } |
174 | } |
107 | } |
175 | 108 | ||
176 | // Schedule the next OCR2A event. The counter is already reset at this time. |
109 | // Schedule the next OCR2A event. The counter is already reset at this time. |
177 | if (remainingPulseTime > 256+128) { |
110 | if (remainingPulseTime > 256+128) { |
178 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
111 | // Set output to reset to zero at next OCR match. It does not really matter when the output is set low again, |
179 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
112 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
180 | OCR2A = 255; |
113 | OCR2A = 255; |
181 | TCCR2A &= ~(1<<COM2A0); |
114 | TCCR2A &= ~(1<<COM2A0); |
182 | remainingPulseTime-=256; |
115 | remainingPulseTime -= 256; |
183 | } else if (remainingPulseTime > 256) { |
116 | } else if (remainingPulseTime > 256) { |
184 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
117 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
185 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
118 | // way, which is in chunks of 256. The remainder would be very small, possibly causing an interrupt on interrupt |
186 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
119 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
187 | remainingPulseTime-=128; |
- | |
188 | OCR2A=127; |
120 | OCR2A=127; |
- | 121 | remainingPulseTime -= 128; |
|
189 | } else { |
122 | } else { |
190 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
123 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
191 | TCCR2A |= (1<<COM2A0); |
124 | TCCR2A |= (1<<COM2A0); |
192 | OCR2A = remainingPulseTime-1; |
125 | OCR2A = remainingPulseTime-1; |
193 | remainingPulseTime=0; |
126 | remainingPulseTime = 0; |
194 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
127 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
195 | } |
128 | } |
196 | } |
129 | } |
197 | 130 |