Rev 1179 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1179 | Rev 1180 | ||
|---|---|---|---|
| Line -... | Line 1... | ||
| - | 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 excample: 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 | // ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
| 1 | #include <avr/io.h> |
51 | #include <avr/io.h> |
| 2 | #include <avr/interrupt.h> |
52 | #include <avr/interrupt.h> |
| 3 | #include "fc.h" |
53 | #include "fc.h" |
| 4 | #include "eeprom.h" |
54 | #include "eeprom.h" |
| 5 | #include "uart.h" |
55 | #include "uart0.h" |
| 6 | #include "main.h" |
56 | #include "main.h" |
| - | 57 | #include "rc.h" |
|
| Line -... | Line 58... | ||
| - | 58 | ||
| 7 | 59 | volatile int16_t ServoNickValue = 0; |
|
| - | 60 | volatile int16_t ServoRollValue = 0; |
|
| Line 8... | Line 61... | ||
| 8 | volatile int16_t ServoValue = 0; |
61 | volatile uint8_t ServoActive = 0; |
| 9 | 62 | ||
| Line 26... | Line 79... | ||
| 26 | // set PD7 as output of the PWM for nick servo |
79 | // set PD7 as output of the PWM for nick servo |
| 27 | DDRD |= (1<<DDD7); |
80 | DDRD |= (1<<DDD7); |
| 28 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
81 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
| Line 29... | Line 82... | ||
| 29 | 82 | ||
| 30 | DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
83 | DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
| - | 84 | //PORTC &= ~(1<<PORTC6); // set PC6 to low |
|
| Line 31... | Line 85... | ||
| 31 | PORTC &= ~(1<<PORTC6); // set PC6 to low |
85 | HEF4017R_ON; // enable reset |
| Line 32... | Line 86... | ||
| 32 | 86 | ||
| 33 | // Timer/Counter 2 Control Register A |
87 | // Timer/Counter 2 Control Register A |
| 34 | 88 | ||
| 35 | // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1) |
89 | // Timer Mode is FastPWM with timer reload at OCR2A (Bits: WGM22 = 1, WGM21 = 1, WGM20 = 1) |
| 36 | // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
90 | // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
| Line 37... | Line 91... | ||
| 37 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
91 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
| Line 38... | Line 92... | ||
| 38 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
92 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
| 39 | TCCR2A |= (1<<WGM21)|(1<<WGM20); |
93 | TCCR2A |= (1<<WGM21)|(1<<WGM20); |
| 40 | 94 | ||
| 41 | // Timer/Counter 2 Control Register B |
95 | // Timer/Counter 2 Control Register B |
| 42 | 96 | ||
| 43 | // Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz |
97 | // Set clock divider for timer 2 to SYSKLOCK/32 = 20MHz / 32 = 625 kHz |
| 44 | // The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us |
98 | // The timer increments from 0x00 to 0xFF with an update rate of 625 kHz or 1.6 us |
| Line 45... | Line 99... | ||
| 45 | // hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms |
99 | // hence the timer overflow interrupt frequency is 625 kHz / 256 = 2.44 kHz or 0.4096 ms |
| 46 | 100 | ||
| Line 47... | Line 101... | ||
| 47 | // divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0) |
101 | // divider 32 (Bits: CS022 = 0, CS21 = 1, CS20 = 1) |
| 48 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22)); |
102 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS22)); |
| - | 103 | TCCR2B |= (1<<CS21)|(1<<CS20)|(1<<WGM22); |
|
| Line 49... | Line 104... | ||
| 49 | TCCR2B |= (1<<CS22); |
104 | |
| 50 | 105 | // Initialize the Timer/Counter 2 Register |
|
| 51 | // Initialize the Timer/Counter 2 Register |
106 | TCNT2 = 0; |
| 52 | TCNT2 = 0; |
107 | |
| Line 53... | Line 108... | ||
| 53 | 108 | // Initialize the Output Compare Register A used for PWM generation on port PD7. |
|
| 54 | // Initialize the Output Compare Register A used for PWM generation on port PD7. |
109 | OCR2A = 255; |
| Line -... | Line 110... | ||
| - | 110 | TCCR2A |= (1<<COM2A1); // set or clear at compare match depends on value of COM2A0 |
|
| - | 111 | ||
| - | 112 | // Timer/Counter 2 Interrupt Mask Register |
|
| - | 113 | // Enable timer output compare match A Interrupt only |
|
| - | 114 | TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
|
| - | 115 | TIMSK2 |= (1<<OCIE2A); |
|
| - | 116 | ||
| - | 117 | SREG = sreg; |
|
| - | 118 | } |
|
| - | 119 | ||
| - | 120 | ||
| 55 | OCR2A = 10; |
121 | void Servo_On(void) |
| 56 | 122 | { |
|
| 57 | // Timer/Counter 2 Interrupt Mask Register |
123 | ServoActive = 1; |
| Line 58... | Line 124... | ||
| 58 | // Enable timer output compare match A Interrupt only |
124 | } |
| 59 | TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
125 | |
| 60 | TIMSK2 |= (1<<OCIE2A); |
- | |
| 61 | - | ||
| 62 | SREG = sreg; |
- | |
| Line -... | Line 126... | ||
| - | 126 | void Servo_Off(void) |
|
| - | 127 | { |
|
| - | 128 | ServoActive = 0; |
|
| - | 129 | HEF4017R_ON; // enable reset |
|
| - | 130 | } |
|
| - | 131 | ||
| - | 132 | /*****************************************************/ |
|
| - | 133 | /* Control Servo Position */ |
|
| - | 134 | /*****************************************************/ |
|
| - | 135 | ||
| - | 136 | ISR(TIMER2_COMPA_vect) |
|
| - | 137 | { |
|
| - | 138 | ||
| - | 139 | // frame len 22.5 ms = 14063 * 1.6 us |
|
| - | 140 | // stop pulse: 0.3 ms = 188 * 1.6 us |
|
| - | 141 | // min servo pulse: 0.6 ms = 375 * 1.6 us |
|
| - | 142 | // max servo pulse: 2.4 ms = 1500 * 1.6 us |
|
| - | 143 | // resolution: 1500 - 375 = 1125 steps |
|
| - | 144 | ||
| 63 | } |
145 | #define IRS_RUNTIME 127 |
| - | 146 | #define PPM_STOPPULSE 188 |
|
| Line -... | Line 147... | ||
| - | 147 | //#define PPM_FRAMELEN 14063 |
|
| 64 | 148 | #define PPM_FRAMELEN (1757 * ParamSet.ServoRefresh) // 22.5 ms / 8 Channels = 2.8125ms per Servo Channel |
|
| 65 | 149 | #define MINSERVOPULSE 375 |
|
| - | 150 | #define MAXSERVOPULSE 1500 |
|
| 66 | /*****************************************************/ |
151 | #define SERVORANGE (MAXSERVOPULSE - MINSERVOPULSE) |
| - | 152 | ||
| - | 153 | static uint8_t PulseOutput = 0; |
|
| 67 | /* Control Servo Position */ |
154 | static uint16_t RemainingPulse = 0; |
| - | 155 | static uint16_t ServoFrameTime = 0; |
|
| 68 | /*****************************************************/ |
156 | static uint8_t ServoIndex = 0; |
| 69 | 157 | ||
| 70 | ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A) |
158 | #define MULTIPLYER 4 |
| - | 159 | static int16_t ServoNickOffset = (255 / 2) * MULTIPLYER; // initial value near center positon |
|
| 71 | { |
160 | |
| 72 | static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match) |
161 | |
| 73 | static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl |
162 | if(BoardRelease < 20) |
| 74 | static uint16_t ServoState = 40; // cycle down counter for this ISR |
163 | { |
| 75 | 164 | //--------------------------- |
|
| 76 | #define MULTIPLIER 4 |
165 | // Nick servo state machine |
| 77 | 166 | //--------------------------- |
|
| 78 | if(BoardRelease < 99) |
167 | if(!PulseOutput) // pulse output complete |
| 79 | { |
168 | { |
| 80 | switch(ServoState) |
169 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
| 81 | { |
170 | { |
| 82 | case 4: |
171 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
| 83 | // recalculate new ServoValue |
172 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
| 84 | ServoValue = 0x0030; // Offset (part 1) |
173 | |
| 85 | FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
174 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
| 86 | ServoValue += FilterServo; // add filtered static offset |
175 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
| 87 | if(ParamSet.ServoNickCompInvert & 0x01) |
176 | if(ParamSet.ServoNickCompInvert & 0x01) |
| 88 | { // inverting movement of servo |
177 | { // inverting movement of servo |
| 89 | ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
178 | ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| 90 | } |
179 | } |
| 91 | else |
- | |
| Line 92... | Line -... | ||
| 92 | { // non inverting movement of servo |
- | |
| 93 | ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
- | |
| 94 | } |
180 | else |
| 95 | // limit servo value to its parameter range definition |
- | |
| 96 | if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) ) |
- | |
| 97 | { |
- | |
| 98 | ServoValue = (int16_t)ParamSet.ServoNickMin * 3; |
- | |
| 99 | } |
- | |
| 100 | else |
- | |
| 101 | if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) ) |
- | |
| 102 | { |
- | |
| 103 | ServoValue = (int16_t)ParamSet.ServoNickMax * 3; |
- | |
| 104 | } |
- | |
| 105 | DebugOut.Analog[20] = ServoValue; |
- | |
| 106 | - | ||
| Line 107... | Line 181... | ||
| 107 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
181 | { // non inverting movement of servo |
| - | 182 | ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
|
| Line -... | Line 183... | ||
| - | 183 | } |
|
| - | 184 | // limit servo value to its parameter range definition |
|
| - | 185 | if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
|
| - | 186 | { |
|
| - | 187 | ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
|
| - | 188 | } |
|
| - | 189 | else |
|
| 108 | if ((ServoValue % 255) < 45) |
190 | if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
| - | 191 | { |
|
| - | 192 | ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
|
| 109 | { // if prepulse width is to short the execution time of this ISR is longer than the next compare match |
193 | } |
| - | 194 | ||
| 110 | // so balance with postpulse width |
195 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
| - | 196 | ||
| - | 197 | ServoNickValue /= MULTIPLYER; |
|
| - | 198 | DebugOut.Analog[20] = ServoNickValue; |
|
| - | 199 | ||
| - | 200 | // range servo pulse width |
|
| - | 201 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
|
| - | 202 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
|
| - | 203 | // accumulate time for correct update rate |
|
| - | 204 | ServoFrameTime = RemainingPulse; |
|
| - | 205 | } |
|
| - | 206 | else // we had a high pulse |
|
| - | 207 | { |
|
| Line 111... | Line 208... | ||
| 111 | ServoValue += 77; |
208 | TCCR2A |= (1<<COM2A0); // make a low pulse |
| 112 | PostPulse = 0x60 - 77; |
209 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; |
| 113 | } |
210 | } |
| 114 | else |
211 | // set pulse output active |
| 115 | { |
212 | PulseOutput = 1; |
| 116 | PostPulse = 0x60; |
213 | } |
| 117 | } |
214 | } // EOF Nick servo state machine |
| 118 | // set output compare register to 255 - prepulse width |
215 | else |
| 119 | OCR2A = 255 - (ServoValue % 256); |
216 | { |
| 120 | // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match) |
217 | //----------------------------------------------------- |
| - | 218 | // PPM state machine, onboard demultiplexed by HEF4017 |
|
| - | 219 | //----------------------------------------------------- |
|
| - | 220 | if(!PulseOutput) // pulse output complete |
|
| 121 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
221 | { |
| - | 222 | if(TCCR2A & (1<<COM2A0)) // we had a low pulse |
|
| - | 223 | { |
|
| - | 224 | TCCR2A &= ~(1<<COM2A0);// make a high pulse |
|
| - | 225 | ||
| - | 226 | if(ServoIndex == 0) // if we are at the sync gap |
|
| - | 227 | { |
|
| - | 228 | RemainingPulse = PPM_FRAMELEN - ServoFrameTime; // generate sync gap by filling time to full frame time |
|
| 122 | 229 | ServoFrameTime = 0; // reset servo frame time |
|
| - | 230 | HEF4017R_ON; // enable HEF4017 reset |
|
| - | 231 | } |
|
| 123 | break; |
232 | else // servo channels |
| - | 233 | { |
|
| 124 | 234 | RemainingPulse = MINSERVOPULSE + SERVORANGE/2; // center position ~ 1.5ms |
|
| 125 | case 3: |
235 | switch(ServoIndex) // map servo channels |
| 126 | case 2: |
236 | { |
| - | 237 | case 1: // Nick Compensation Servo |
|
| 127 | case 1: |
238 | ServoNickOffset = (ServoNickOffset * 3 + (int16_t)FCParam.ServoNickControl * MULTIPLYER) / 4; // lowpass offset |
| - | 239 | ServoNickValue = ServoNickOffset; // offset (Range from 0 to 255 * 3 = 765) |
|
| - | 240 | if(ParamSet.ServoNickCompInvert & 0x01) |
|
| 128 | 241 | { // inverting movement of servo |
|
| - | 242 | ServoNickValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
|
| - | 243 | } |
|
| - | 244 | else |
|
| 129 | if(ServoValue > 255) // is larger than a full timer 2 cycle |
245 | { // non inverting movement of servo |
| 130 | { |
246 | ServoNickValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * MULTIPLYER * (IntegralGyroNick / 128L ) ) / (256L) ); |
| - | 247 | } |
|
| - | 248 | // limit servo value to its parameter range definition |
|
| 131 | PORTD |= (1<<PORTD7); // set PD7 to high |
249 | if(ServoNickValue < ((int16_t)ParamSet.ServoNickMin * MULTIPLYER) ) |
| - | 250 | { |
|
| 132 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
251 | ServoNickValue = (int16_t)ParamSet.ServoNickMin * MULTIPLYER; |
| - | 252 | } |
|
| - | 253 | else |
|
| - | 254 | if(ServoNickValue > ((int16_t)ParamSet.ServoNickMax * MULTIPLYER) ) |
|
| - | 255 | { |
|
| - | 256 | ServoNickValue = (int16_t)ParamSet.ServoNickMax * MULTIPLYER; |
|
| - | 257 | } |
|
| 133 | ServoValue -= 255; // substract full timer cycle |
258 | |
| - | 259 | RemainingPulse += ServoNickValue - (256 / 2) * MULTIPLYER; // shift ServoNickValue to center position |
|
| - | 260 | ||
| - | 261 | ServoNickValue /= MULTIPLYER; |
|
| - | 262 | DebugOut.Analog[20] = ServoNickValue; |
|
| - | 263 | break; |
|
| - | 264 | ||
| - | 265 | default: // other servo channels |
|
| - | 266 | RemainingPulse += 2 * PPM_in[ServoIndex]; // add channel value, factor of 2 because timer 1 increments 3.2µs |
|
| - | 267 | break; |
|
| - | 268 | } |
|
| - | 269 | // range servo pulse width |
|
| - | 270 | if(RemainingPulse > MAXSERVOPULSE ) RemainingPulse = MAXSERVOPULSE; // upper servo pulse limit |
|
| - | 271 | else if(RemainingPulse < MINSERVOPULSE ) RemainingPulse = MINSERVOPULSE; // lower servo pulse limit |
|
| - | 272 | // substract stop pulse width |
|
| 134 | } |
273 | RemainingPulse -= PPM_STOPPULSE; |
| 135 | else // the post pule must be generated |
274 | // accumulate time for correct sync gap |
| - | 275 | ServoFrameTime += RemainingPulse; |
|
| 136 | { |
276 | } |
| 137 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode |
277 | } |
| 138 | OCR2A = PostPulse; // Offset Part2 |
278 | else // we had a high pulse |
| - | 279 | { |
|
| 139 | ServoState = 1; // jump to ServoState 0 with next ISR call |
280 | TCCR2A |= (1<<COM2A0); // make a low pulse |
| 140 | } |
281 | // set pulsewidth to stop pulse width |
| - | 282 | RemainingPulse = PPM_STOPPULSE; |
|
| - | 283 | // accumulate time for correct sync gap |
|
| - | 284 | ServoFrameTime += RemainingPulse; |
|
| - | 285 | if(ServoActive && RC_Quality > 180) HEF4017R_OFF; // disable HEF4017 reset |
|
| - | 286 | ServoIndex++; // change to next servo channel |
|
| - | 287 | if(ServoIndex > ParamSet.ServoRefresh) ServoIndex = 0; // reset to the sync gap |
|
| - | 288 | } |
|
| - | 289 | // set pulse output active |
|
| 141 | break; |
290 | PulseOutput = 1; |
| - | 291 | } |
|
| - | 292 | } // EOF PPM state machine |
|
| - | 293 | ||
| - | 294 | // General pulse output generator |
|
| - | 295 | if(RemainingPulse > (255 + IRS_RUNTIME)) |
|
| - | 296 | { |
|
| - | 297 | OCR2A = 255; |
|
| - | 298 | RemainingPulse -= 255; |
|
| - | 299 | } |
|
| - | 300 | else |
|
| - | 301 | { |
|
| - | 302 | if(RemainingPulse > 255) // this is the 2nd last part |
|
| - | 303 | { |
|
| - | 304 | if((RemainingPulse - 255) < IRS_RUNTIME) |
|
| Line -... | Line 305... | ||
| - | 305 | { |
|