Rev 2052 | Rev 2067 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2052 | Rev 2062 | ||
---|---|---|---|
1 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
2 | // + Copyright (c) 04.2007 Holger Buss |
- | |
3 | // + Nur für den privaten Gebrauch |
- | |
4 | // + www.MikroKopter.com |
- | |
5 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
6 | // + Es gilt für das gesamte Projekt (Hardware, Software, Binärfiles, Sourcecode und Dokumentation), |
- | |
7 | // + dass eine Nutzung (auch auszugsweise) nur für den privaten (nicht-kommerziellen) Gebrauch zulässig ist. |
- | |
8 | // + Sollten direkte oder indirekte kommerzielle Absichten verfolgt werden, ist mit uns (info@mikrokopter.de) Kontakt |
- | |
9 | // + bzgl. der Nutzungsbedingungen aufzunehmen. |
- | |
10 | // + Eine kommerzielle Nutzung ist z.B.Verkauf von MikroKoptern, Bestückung und Verkauf von Platinen oder Bausätzen, |
- | |
11 | // + Verkauf von Luftbildaufnahmen, usw. |
- | |
12 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
13 | // + Werden Teile des Quellcodes (mit oder ohne Modifikation) weiterverwendet oder veröffentlicht, |
- | |
14 | // + unterliegen sie auch diesen Nutzungsbedingungen und diese Nutzungsbedingungen incl. Copyright müssen dann beiliegen |
- | |
15 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
16 | // + Sollte die Software (auch auszugesweise) oder sonstige Informationen des MikroKopter-Projekts |
- | |
17 | // + auf anderen Webseiten oder sonstigen Medien veröffentlicht werden, muss unsere Webseite "http://www.mikrokopter.de" |
- | |
18 | // + eindeutig als Ursprung verlinkt werden |
- | |
19 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
20 | // + Keine Gewähr auf Fehlerfreiheit, Vollständigkeit oder Funktion |
- | |
21 | // + Benutzung auf eigene Gefahr |
- | |
22 | // + Wir übernehmen keinerlei Haftung für direkte oder indirekte Personen- oder Sachschäden |
- | |
23 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
24 | // + Die Portierung der Software (oder Teile davon) auf andere Systeme (ausser der Hardware von www.mikrokopter.de) ist nur |
- | |
25 | // + mit unserer Zustimmung zulässig |
- | |
26 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
27 | // + Die Funktion printf_P() unterliegt ihrer eigenen Lizenz und ist hiervon nicht betroffen |
- | |
28 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
29 | // + Redistributions of source code (with or without modifications) must retain the above copyright notice, |
- | |
30 | // + this list of conditions and the following disclaimer. |
- | |
31 | // + * Neither the name of the copyright holders nor the names of contributors may be used to endorse or promote products derived |
- | |
32 | // + from this software without specific prior written permission. |
- | |
33 | // + * The use of this project (hardware, software, binary files, sources and documentation) is only permittet |
- | |
34 | // + for non-commercial use (directly or indirectly) |
- | |
35 | // + Commercial use (for example: selling of MikroKopters, selling of PCBs, assembly, ...) is only permitted |
- | |
36 | // + with our written permission |
- | |
37 | // + * If sources or documentations are redistributet on other webpages, out webpage (http://www.MikroKopter.de) must be |
- | |
38 | // + clearly linked as origin |
- | |
39 | // + * porting to systems other than hardware from www.mikrokopter.de is not allowed |
- | |
40 | // + THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" |
- | |
41 | // + AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE |
- | |
42 | // + IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE |
- | |
43 | // + ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE |
- | |
44 | // + LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR |
- | |
45 | // + CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF |
- | |
46 | // + SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS |
- | |
47 | // + INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN// + CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) |
- | |
48 | // + ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE |
- | |
49 | // + POSSIBILITY OF SUCH DAMAGE. |
- | |
50 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | |
51 | #include <avr/io.h> |
1 | #include <avr/io.h> |
52 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
53 | #include "eeprom.h" |
3 | #include "eeprom.h" |
54 | #include "rc.h" |
4 | #include "rc.h" |
55 | #include "attitude.h" |
5 | #include "attitude.h" |
56 | 6 | ||
57 | #define COARSERESOLUTION 1 |
7 | //#define COARSERESOLUTION 1 |
58 | 8 | ||
59 | #ifdef COARSERESOLUTION |
9 | #ifdef COARSERESOLUTION |
60 | #define NEUTRAL_PULSELENGTH 938 |
10 | #define NEUTRAL_PULSELENGTH 938 |
61 | #define SERVOLIMIT 500 |
11 | #define SERVOLIMIT 500 |
62 | #define SCALE_FACTOR 4 |
12 | #define SCALE_FACTOR 4 |
63 | #define CS2 ((1<<CS21)|(1<<CS20)) |
13 | #define CS2 ((1<<CS21)|(1<<CS20)) |
- | 14 | ||
64 | #else |
15 | #else |
- | 16 | ||
65 | #define NEUTRAL_PULSELENGTH 3750 |
17 | #define NEUTRAL_PULSELENGTH 3750 |
66 | #define SERVOLIMIT 2000 |
18 | #define SERVOLIMIT 2000 |
67 | #define SCALE_FACTOR 16 |
19 | #define SCALE_FACTOR 16 |
68 | #define CS2 (<<CS21) |
20 | #define CS2 (1<<CS21) |
69 | #endif |
21 | #endif |
70 | 22 | ||
71 | #define MAX_SERVOS 8 |
23 | #define MAX_SERVOS 8 |
72 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
24 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
73 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
25 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
74 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
26 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
75 | 27 | ||
76 | //volatile uint8_t servoActive = 0; |
28 | //volatile uint8_t servoActive = 0; |
77 | volatile uint8_t recalculateServoTimes = 0; |
29 | volatile uint8_t recalculateServoTimes = 0; |
78 | volatile uint16_t servoValues[MAX_SERVOS]; |
30 | volatile uint16_t servoValues[MAX_SERVOS]; |
79 | volatile uint16_t previousManualValues[2]; |
31 | volatile uint16_t previousManualValues[2]; |
80 | 32 | ||
81 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
33 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
82 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
34 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
83 | 35 | ||
84 | /***************************************************** |
36 | /***************************************************** |
85 | * Initialize Timer 2 |
37 | * Initialize Timer 2 |
86 | *****************************************************/ |
38 | *****************************************************/ |
87 | void timer2_init(void) { |
39 | void timer2_init(void) { |
88 | uint8_t sreg = SREG; |
40 | uint8_t sreg = SREG; |
89 | 41 | ||
90 | // disable all interrupts before reconfiguration |
42 | // disable all interrupts before reconfiguration |
91 | cli(); |
43 | cli(); |
92 | 44 | ||
93 | // set PD7 as output of the PWM for pitch servo |
45 | // set PD7 as output of the PWM for pitch servo |
94 | DDRD |= (1 << DDD7); |
46 | DDRD |= (1 << DDD7); |
95 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
47 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
96 | 48 | ||
97 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
49 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
98 | HEF4017R_ON; // enable reset |
50 | HEF4017R_ON; // enable reset |
99 | 51 | ||
100 | // Timer/Counter 2 Control Register A |
52 | // Timer/Counter 2 Control Register A |
101 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
53 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
102 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
54 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
103 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
55 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
104 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
56 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
105 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
57 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
106 | 58 | ||
107 | // Timer/Counter 2 Control Register B |
59 | // Timer/Counter 2 Control Register B |
108 | 60 | ||
109 | // 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 |
110 | // 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 |
111 | // 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 |
112 | 64 | ||
113 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
65 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
114 | TCCR2B |= CS2; |
66 | TCCR2B |= CS2; |
115 | 67 | ||
116 | // Initialize the Timer/Counter 2 Register |
68 | // Initialize the Timer/Counter 2 Register |
117 | TCNT2 = 0; |
69 | TCNT2 = 0; |
118 | 70 | ||
119 | // 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. |
120 | OCR2A = 255; |
72 | OCR2A = 255; |
121 | 73 | ||
122 | // Timer/Counter 2 Interrupt Mask Register |
74 | // Timer/Counter 2 Interrupt Mask Register |
123 | // Enable timer output compare match A Interrupt only |
75 | // Enable timer output compare match A Interrupt only |
124 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
76 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
125 | TIMSK2 |= (1 << OCIE2A); |
77 | TIMSK2 |= (1 << OCIE2A); |
126 | 78 | ||
127 | for (uint8_t axis=0; axis<2; axis++) |
79 | for (uint8_t axis=0; axis<2; axis++) |
128 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
80 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
129 | 81 | ||
130 | SREG = sreg; |
82 | SREG = sreg; |
131 | } |
83 | } |
132 | 84 | ||
133 | /* |
85 | /* |
134 | void servo_On(void) { |
86 | void servo_On(void) { |
135 | servoActive = 1; |
87 | servoActive = 1; |
136 | } |
88 | } |
137 | void servo_Off(void) { |
89 | void servo_Off(void) { |
138 | servoActive = 0; |
90 | servoActive = 0; |
139 | HEF4017R_ON; // enable reset |
91 | HEF4017R_ON; // enable reset |
140 | } |
92 | } |
141 | */ |
93 | */ |
142 | 94 | ||
143 | /***************************************************** |
95 | /***************************************************** |
144 | * Control Servo Position |
96 | * Control Servo Position |
145 | *****************************************************/ |
97 | *****************************************************/ |
146 | 98 | ||
147 | /*typedef struct { |
99 | /*typedef struct { |
148 | uint8_t manualControl; |
100 | uint8_t manualControl; |
149 | uint8_t compensationFactor; |
101 | uint8_t compensationFactor; |
150 | uint8_t minValue; |
102 | uint8_t minValue; |
151 | uint8_t maxValue; |
103 | uint8_t maxValue; |
152 | uint8_t flags; |
104 | uint8_t flags; |
153 | } servo_t;*/ |
105 | } servo_t;*/ |
154 | 106 | ||
155 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
107 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
156 | int32_t value = attitude[axis] / 64L; // between -500000 to 500000 extreme limits. Just about |
108 | int32_t value = attitude[axis] / 64L; // between -500000 to 500000 extreme limits. Just about |
157 | // 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. |
158 | // 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. |
159 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
111 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
160 | value /= 256L; |
112 | value /= 256L; |
161 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
113 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
162 | return -value; |
114 | return -value; |
163 | return value; |
115 | return value; |
164 | } |
116 | } |
165 | 117 | ||
166 | // With constant-speed limitation. |
118 | // With constant-speed limitation. |
167 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
119 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
168 | int16_t diff = manualValue - previousManualValues[axis]; |
120 | int16_t diff = manualValue - previousManualValues[axis]; |
169 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
121 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
170 | if (diff > maxSpeed) diff = maxSpeed; |
122 | if (diff > maxSpeed) diff = maxSpeed; |
171 | else if (diff < -maxSpeed) diff = -maxSpeed; |
123 | else if (diff < -maxSpeed) diff = -maxSpeed; |
172 | manualValue = previousManualValues[axis] + diff; |
124 | manualValue = previousManualValues[axis] + diff; |
173 | previousManualValues[axis] = manualValue; |
125 | previousManualValues[axis] = manualValue; |
174 | return manualValue; |
126 | return manualValue; |
175 | } |
127 | } |
176 | 128 | ||
177 | // add stabilization and manual, apply soft position limits. |
129 | // add stabilization and manual, apply soft position limits. |
178 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
130 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
179 | int16_t featuredServoValue(uint8_t axis) { |
131 | int16_t featuredServoValue(uint8_t axis) { |
180 | int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
132 | int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
181 | value += calculateStabilizedServoAxis(axis); |
133 | value += calculateStabilizedServoAxis(axis); |
182 | int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
134 | int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
183 | if (value < limit) value = limit; |
135 | if (value < limit) value = limit; |
184 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
136 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
185 | if (value > limit) value = limit; |
137 | if (value > limit) value = limit; |
186 | return value; |
138 | return value; |
187 | } |
139 | } |
188 | 140 | ||
189 | uint16_t servoValue(uint8_t axis) { |
141 | uint16_t servoValue(uint8_t axis) { |
190 | int16_t value; |
142 | int16_t value; |
191 | if (axis<2) value = featuredServoValue(axis); |
143 | if (axis<2) value = featuredServoValue(axis); |
192 | 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. |
193 | // Shift out of the [0..255*SCALE_FACTOR] space |
145 | // Shift out of the [0..255*SCALE_FACTOR] space |
194 | value -= (128 * SCALE_FACTOR); |
146 | value -= (128 * SCALE_FACTOR); |
195 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
147 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
196 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
148 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
197 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
149 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
198 | return value + NEUTRAL_PULSELENGTH; |
150 | return value + NEUTRAL_PULSELENGTH; |
199 | } |
151 | } |
200 | 152 | ||
201 | void calculateServoValues(void) { |
153 | void calculateServoValues(void) { |
202 | if (!recalculateServoTimes) return; |
154 | if (!recalculateServoTimes) return; |
203 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
155 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
204 | servoValues[axis] = servoValue(axis); |
156 | servoValues[axis] = servoValue(axis); |
205 | } |
157 | } |
206 | recalculateServoTimes = 0; |
158 | recalculateServoTimes = 0; |
207 | } |
159 | } |
208 | 160 | ||
209 | ISR(TIMER2_COMPA_vect) { |
161 | ISR(TIMER2_COMPA_vect) { |
210 | static uint16_t remainingPulseTime; |
162 | static uint16_t remainingPulseTime; |
211 | static uint8_t servoIndex = 0; |
163 | static uint8_t servoIndex = 0; |
212 | static uint16_t sumOfPulseTimes = 0; |
164 | static uint16_t sumOfPulseTimes = 0; |
213 | 165 | ||
214 | if (!remainingPulseTime) { |
166 | if (!remainingPulseTime) { |
215 | // 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. |
216 | if (servoIndex < staticParams.servoCount) { |
168 | if (servoIndex < staticParams.servoCount) { |
217 | // There are more signals to output. |
169 | // There are more signals to output. |
218 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
170 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
219 | servoIndex++; |
171 | servoIndex++; |
220 | } else { |
172 | } else { |
221 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
173 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
222 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
174 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
223 | sumOfPulseTimes = servoIndex = 0; |
175 | sumOfPulseTimes = servoIndex = 0; |
224 | recalculateServoTimes = 1; |
176 | recalculateServoTimes = 1; |
225 | HEF4017R_ON; |
177 | HEF4017R_ON; |
226 | } |
178 | } |
227 | } |
179 | } |
228 | 180 | ||
229 | // 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. |
230 | if (remainingPulseTime > 256+128) { |
182 | if (remainingPulseTime > 256+128) { |
231 | // 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, |
232 | // 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. |
233 | OCR2A = 255; |
185 | OCR2A = 255; |
234 | TCCR2A &= ~(1<<COM2A0); |
186 | TCCR2A &= ~(1<<COM2A0); |
235 | remainingPulseTime-=256; |
187 | remainingPulseTime-=256; |
236 | } else if (remainingPulseTime > 256) { |
188 | } else if (remainingPulseTime > 256) { |
237 | // 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 |
238 | // 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 |
239 | // 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. |
240 | remainingPulseTime-=128; |
192 | remainingPulseTime-=128; |
241 | OCR2A=127; |
193 | OCR2A=127; |
242 | } else { |
194 | } else { |
243 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
195 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
244 | TCCR2A |= (1<<COM2A0); |
196 | TCCR2A |= (1<<COM2A0); |
245 | OCR2A = remainingPulseTime-1; |
197 | OCR2A = remainingPulseTime-1; |
246 | remainingPulseTime=0; |
198 | remainingPulseTime=0; |
247 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
199 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
248 | } |
200 | } |
249 | } |
201 | } |
250 | 202 |