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