Rev 2048 | Rev 2062 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 2048 | Rev 2052 | ||
---|---|---|---|
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" |
- | |
55 | #include "rc.h" |
54 | #include "rc.h" |
56 | #include "attitude.h" |
55 | #include "attitude.h" |
57 | 56 | ||
58 | #define COARSERESOLUTION 1 |
57 | #define COARSERESOLUTION 1 |
59 | 58 | ||
60 | #ifdef COARSERESOLUTION |
59 | #ifdef COARSERESOLUTION |
61 | #define NEUTRAL_PULSELENGTH 938 |
60 | #define NEUTRAL_PULSELENGTH 938 |
62 | #define SERVOLIMIT 500 |
61 | #define SERVOLIMIT 500 |
63 | #define SCALE_FACTOR 4 |
62 | #define SCALE_FACTOR 4 |
64 | #define CS2 ((1<<CS21)|(1<<CS20)) |
63 | #define CS2 ((1<<CS21)|(1<<CS20)) |
65 | #else |
64 | #else |
66 | #define NEUTRAL_PULSELENGTH 3750 |
65 | #define NEUTRAL_PULSELENGTH 3750 |
67 | #define SERVOLIMIT 2000 |
66 | #define SERVOLIMIT 2000 |
68 | #define SCALE_FACTOR 16 |
67 | #define SCALE_FACTOR 16 |
69 | #define CS2 (<<CS21) |
68 | #define CS2 (<<CS21) |
70 | #endif |
69 | #endif |
71 | 70 | ||
72 | #define MAX_SERVOS 8 |
71 | #define MAX_SERVOS 8 |
73 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
72 | #define FRAMELEN ((NEUTRAL_PULSELENGTH + SERVOLIMIT) * staticParams.servoCount + 128) |
74 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
73 | #define MIN_PULSELENGTH (NEUTRAL_PULSELENGTH - SERVOLIMIT) |
75 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
74 | #define MAX_PULSELENGTH (NEUTRAL_PULSELENGTH + SERVOLIMIT) |
76 | 75 | ||
77 | //volatile uint8_t servoActive = 0; |
76 | //volatile uint8_t servoActive = 0; |
78 | volatile uint8_t recalculateServoTimes = 0; |
77 | volatile uint8_t recalculateServoTimes = 0; |
79 | volatile uint16_t servoValues[MAX_SERVOS]; |
78 | volatile uint16_t servoValues[MAX_SERVOS]; |
80 | volatile uint16_t previousManualValues[2]; |
79 | volatile uint16_t previousManualValues[2]; |
81 | 80 | ||
82 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
81 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
83 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
82 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
84 | 83 | ||
85 | /***************************************************** |
84 | /***************************************************** |
86 | * Initialize Timer 2 |
85 | * Initialize Timer 2 |
87 | *****************************************************/ |
86 | *****************************************************/ |
88 | void timer2_init(void) { |
87 | void timer2_init(void) { |
89 | uint8_t sreg = SREG; |
88 | uint8_t sreg = SREG; |
90 | 89 | ||
91 | // disable all interrupts before reconfiguration |
90 | // disable all interrupts before reconfiguration |
92 | cli(); |
91 | cli(); |
93 | 92 | ||
94 | // set PD7 as output of the PWM for pitch servo |
93 | // set PD7 as output of the PWM for pitch servo |
95 | DDRD |= (1 << DDD7); |
94 | DDRD |= (1 << DDD7); |
96 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
95 | PORTD &= ~(1 << PORTD7); // set PD7 to low |
97 | 96 | ||
98 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
97 | DDRC |= (1 << DDC6); // set PC6 as output (Reset for HEF4017) |
99 | HEF4017R_ON; // enable reset |
98 | HEF4017R_ON; // enable reset |
100 | 99 | ||
101 | // Timer/Counter 2 Control Register A |
100 | // Timer/Counter 2 Control Register A |
102 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
101 | // Timer Mode is CTC (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 0) |
103 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
102 | // PD7: Output OCR2 match, (Bits: COM2A1 = 1, COM2A0 = 0) |
104 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
103 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
105 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
104 | TCCR2A &= ~((1 << COM2A0) | (1 << COM2B1) | (1 << COM2B0) | (1 << WGM20) | (1 << WGM22)); |
106 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
105 | TCCR2A |= (1 << COM2A1) | (1 << WGM21); |
107 | 106 | ||
108 | // Timer/Counter 2 Control Register B |
107 | // Timer/Counter 2 Control Register B |
109 | 108 | ||
110 | // Set clock divider for timer 2 to 20MHz / 8 = 2.5 MHz |
109 | // 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 |
110 | // 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 |
111 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 9.765 kHz or 0.1024ms |
113 | 112 | ||
114 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
113 | TCCR2B &= ~((1 << FOC2A) | (1 << FOC2B) | (1 << CS20) | (1 << CS21) | (1 << CS22)); |
115 | TCCR2B |= CS2; |
114 | TCCR2B |= CS2; |
116 | 115 | ||
117 | // Initialize the Timer/Counter 2 Register |
116 | // Initialize the Timer/Counter 2 Register |
118 | TCNT2 = 0; |
117 | TCNT2 = 0; |
119 | 118 | ||
120 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
119 | // Initialize the Output Compare Register A used for signal generation on port PD7. |
121 | OCR2A = 255; |
120 | OCR2A = 255; |
122 | 121 | ||
123 | // Timer/Counter 2 Interrupt Mask Register |
122 | // Timer/Counter 2 Interrupt Mask Register |
124 | // Enable timer output compare match A Interrupt only |
123 | // Enable timer output compare match A Interrupt only |
125 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
124 | TIMSK2 &= ~((1 << OCIE2B) | (1 << TOIE2)); |
126 | TIMSK2 |= (1 << OCIE2A); |
125 | TIMSK2 |= (1 << OCIE2A); |
127 | 126 | ||
128 | for (uint8_t axis=0; axis<2; axis++) |
127 | for (uint8_t axis=0; axis<2; axis++) |
129 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
128 | previousManualValues[axis] = dynamicParams.servoManualControl[axis] * SCALE_FACTOR; |
130 | 129 | ||
131 | SREG = sreg; |
130 | SREG = sreg; |
132 | } |
131 | } |
133 | 132 | ||
134 | /* |
133 | /* |
135 | void servo_On(void) { |
134 | void servo_On(void) { |
136 | servoActive = 1; |
135 | servoActive = 1; |
137 | } |
136 | } |
138 | void servo_Off(void) { |
137 | void servo_Off(void) { |
139 | servoActive = 0; |
138 | servoActive = 0; |
140 | HEF4017R_ON; // enable reset |
139 | HEF4017R_ON; // enable reset |
141 | } |
140 | } |
142 | */ |
141 | */ |
143 | 142 | ||
144 | /***************************************************** |
143 | /***************************************************** |
145 | * Control Servo Position |
144 | * Control Servo Position |
146 | *****************************************************/ |
145 | *****************************************************/ |
147 | 146 | ||
148 | /*typedef struct { |
147 | /*typedef struct { |
149 | uint8_t manualControl; |
148 | uint8_t manualControl; |
150 | uint8_t compensationFactor; |
149 | uint8_t compensationFactor; |
151 | uint8_t minValue; |
150 | uint8_t minValue; |
152 | uint8_t maxValue; |
151 | uint8_t maxValue; |
153 | uint8_t flags; |
152 | uint8_t flags; |
154 | } servo_t;*/ |
153 | } servo_t;*/ |
155 | 154 | ||
156 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
155 | int16_t calculateStabilizedServoAxis(uint8_t axis) { |
157 | int32_t value = attitude[axis] / 64L; // between -500000 to 500000 extreme limits. Just about |
156 | 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. |
157 | // 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. |
158 | // That is a divisor of about 1<<14. Same conclusion as H&I. |
160 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
159 | value *= staticParams.servoConfigurations[axis].stabilizationFactor; |
161 | value /= 256L; |
160 | value /= 256L; |
162 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
161 | if (staticParams.servoConfigurations[axis].flags & SERVO_STABILIZATION_REVERSE) |
163 | return -value; |
162 | return -value; |
164 | return value; |
163 | return value; |
165 | } |
164 | } |
166 | 165 | ||
167 | // With constant-speed limitation. |
166 | // With constant-speed limitation. |
168 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
167 | uint16_t calculateManualServoAxis(uint8_t axis, uint16_t manualValue) { |
169 | int16_t diff = manualValue - previousManualValues[axis]; |
168 | int16_t diff = manualValue - previousManualValues[axis]; |
170 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
169 | uint8_t maxSpeed = staticParams.servoManualMaxSpeed; |
171 | if (diff > maxSpeed) diff = maxSpeed; |
170 | if (diff > maxSpeed) diff = maxSpeed; |
172 | else if (diff < -maxSpeed) diff = -maxSpeed; |
171 | else if (diff < -maxSpeed) diff = -maxSpeed; |
173 | manualValue = previousManualValues[axis] + diff; |
172 | manualValue = previousManualValues[axis] + diff; |
174 | previousManualValues[axis] = manualValue; |
173 | previousManualValues[axis] = manualValue; |
175 | return manualValue; |
174 | return manualValue; |
176 | } |
175 | } |
177 | 176 | ||
178 | // add stabilization and manual, apply soft position limits. |
177 | // add stabilization and manual, apply soft position limits. |
179 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
178 | // All in a [0..255*SCALE_FACTOR] space (despite signed types used internally) |
180 | int16_t featuredServoValue(uint8_t axis) { |
179 | int16_t featuredServoValue(uint8_t axis) { |
181 | int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
180 | int16_t value = calculateManualServoAxis(axis, dynamicParams.servoManualControl[axis] * SCALE_FACTOR); |
182 | value += calculateStabilizedServoAxis(axis); |
181 | value += calculateStabilizedServoAxis(axis); |
183 | int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
182 | int16_t limit = staticParams.servoConfigurations[axis].minValue * SCALE_FACTOR; |
184 | if (value < limit) value = limit; |
183 | if (value < limit) value = limit; |
185 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
184 | limit = staticParams.servoConfigurations[axis].maxValue * SCALE_FACTOR; |
186 | if (value > limit) value = limit; |
185 | if (value > limit) value = limit; |
187 | return value; |
186 | return value; |
188 | } |
187 | } |
189 | 188 | ||
190 | uint16_t servoValue(uint8_t axis) { |
189 | uint16_t servoValue(uint8_t axis) { |
191 | int16_t value; |
190 | int16_t value; |
192 | if (axis<2) value = featuredServoValue(axis); |
191 | if (axis<2) value = featuredServoValue(axis); |
193 | else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
192 | else value = 128 * SCALE_FACTOR; // dummy. Replace by something useful for servos 3..8. |
194 | // Shift out of the [0..255*SCALE_FACTOR] space |
193 | // Shift out of the [0..255*SCALE_FACTOR] space |
195 | value -= (128 * SCALE_FACTOR); |
194 | value -= (128 * SCALE_FACTOR); |
196 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
195 | if (value < -SERVOLIMIT) value = -SERVOLIMIT; |
197 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
196 | else if (value > SERVOLIMIT) value = SERVOLIMIT; |
198 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
197 | // Shift into the [NEUTRAL_PULSELENGTH-SERVOLIMIT..NEUTRAL_PULSELENGTH+SERVOLIMIT] space. |
199 | return value + NEUTRAL_PULSELENGTH; |
198 | return value + NEUTRAL_PULSELENGTH; |
200 | } |
199 | } |
201 | 200 | ||
202 | void calculateServoValues(void) { |
201 | void calculateServoValues(void) { |
203 | if (!recalculateServoTimes) return; |
202 | if (!recalculateServoTimes) return; |
204 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
203 | for (uint8_t axis=0; axis<MAX_SERVOS; axis++) { |
205 | servoValues[axis] = servoValue(axis); |
204 | servoValues[axis] = servoValue(axis); |
206 | } |
205 | } |
207 | recalculateServoTimes = 0; |
206 | recalculateServoTimes = 0; |
208 | } |
207 | } |
209 | 208 | ||
210 | ISR(TIMER2_COMPA_vect) { |
209 | ISR(TIMER2_COMPA_vect) { |
211 | static uint16_t remainingPulseTime; |
210 | static uint16_t remainingPulseTime; |
212 | static uint8_t servoIndex = 0; |
211 | static uint8_t servoIndex = 0; |
213 | static uint16_t sumOfPulseTimes = 0; |
212 | static uint16_t sumOfPulseTimes = 0; |
214 | 213 | ||
215 | if (!remainingPulseTime) { |
214 | if (!remainingPulseTime) { |
216 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
215 | // Pulse is over, and the next pulse has already just started. Calculate length of next pulse. |
217 | if (servoIndex < staticParams.servoCount) { |
216 | if (servoIndex < staticParams.servoCount) { |
218 | // There are more signals to output. |
217 | // There are more signals to output. |
219 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
218 | sumOfPulseTimes += (remainingPulseTime = servoValues[servoIndex]); |
220 | servoIndex++; |
219 | servoIndex++; |
221 | } else { |
220 | } else { |
222 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
221 | // There are no more signals. Reset the counter and make this pulse cover the missing frame time. |
223 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
222 | remainingPulseTime = FRAMELEN - sumOfPulseTimes; |
224 | sumOfPulseTimes = servoIndex = 0; |
223 | sumOfPulseTimes = servoIndex = 0; |
225 | recalculateServoTimes = 1; |
224 | recalculateServoTimes = 1; |
226 | HEF4017R_ON; |
225 | HEF4017R_ON; |
227 | } |
226 | } |
228 | } |
227 | } |
229 | 228 | ||
230 | // Schedule the next OCR2A event. The counter is already reset at this time. |
229 | // Schedule the next OCR2A event. The counter is already reset at this time. |
231 | if (remainingPulseTime > 256+128) { |
230 | 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, |
231 | // 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. |
232 | // as long as it happens once per pulse. This will, because all pulses are > 255+128 long. |
234 | OCR2A = 255; |
233 | OCR2A = 255; |
235 | TCCR2A &= ~(1<<COM2A0); |
234 | TCCR2A &= ~(1<<COM2A0); |
236 | remainingPulseTime-=256; |
235 | remainingPulseTime-=256; |
237 | } else if (remainingPulseTime > 256) { |
236 | } else if (remainingPulseTime > 256) { |
238 | // Remaining pulse lengths in the range [256..256+128] might cause trouble if handled the standard |
237 | // 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 |
238 | // 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. |
239 | // condition. Instead we now make a chunk of 128. The remaining chunk will then be in [128..255] which is OK. |
241 | remainingPulseTime-=128; |
240 | remainingPulseTime-=128; |
242 | OCR2A=127; |
241 | OCR2A=127; |
243 | } else { |
242 | } else { |
244 | // Set output to high at next OCR match. This is when the 4017 counter will advance by one. Also set reset low |
243 | // 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); |
244 | TCCR2A |= (1<<COM2A0); |
246 | OCR2A = remainingPulseTime-1; |
245 | OCR2A = remainingPulseTime-1; |
247 | remainingPulseTime=0; |
246 | remainingPulseTime=0; |
248 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
247 | HEF4017R_OFF; // implement servo-disable here, by only removing the reset signal if ServoEnabled!=0. |
249 | } |
248 | } |
250 | } |
249 | } |
251 | 250 |