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