Rev 935 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 935 | Rev 936 | ||
|---|---|---|---|
| Line 1... | Line 1... | ||
| 1 | #include <avr/io.h> |
1 | #include <avr/io.h> |
| 2 | #include <avr/interrupt.h> |
2 | #include <avr/interrupt.h> |
| 3 | #include "fc.h" |
3 | #include "fc.h" |
| 4 | #include "eeprom.h" |
4 | #include "eeprom.h" |
| - | 5 | #include "uart.h" |
|
| - | 6 | ||
| - | 7 | volatile uint16_t ServoValue = 0; |
|
| Line 5... | Line -... | ||
| 5 | - | ||
| Line 6... | Line 8... | ||
| 6 | volatile int16_t ServoValue = 0; |
8 | |
| 7 | 9 | ||
| 8 | 10 | ||
| Line 18... | Line 20... | ||
| 18 | // disable all interrupts before reconfiguration |
20 | // disable all interrupts before reconfiguration |
| 19 | cli(); |
21 | cli(); |
| Line 20... | Line 22... | ||
| 20 | 22 | ||
| 21 | // set PD7 as output of the PWM for nick servo |
23 | // set PD7 as output of the PWM for nick servo |
| 22 | DDRD |=(1<<DDD7); |
24 | DDRD |=(1<<DDD7); |
| Line 23... | Line 25... | ||
| 23 | PORTD |= (1<<PORTD7); |
25 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
| Line 24... | Line 26... | ||
| 24 | 26 | ||
| 25 | 27 | ||
| 26 | // Timer/Counter 2 Control Register A |
28 | // Timer/Counter 2 Control Register A |
| 27 | 29 | ||
| 28 | // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1) |
30 | // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1) |
| Line 29... | Line 31... | ||
| 29 | // PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, non inverting PWM (Bits: COM2A1 = 1, COM2A0 = 0) |
31 | // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
| Line 30... | Line 32... | ||
| 30 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
32 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
| 31 | TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0)); |
33 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
| 32 | TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20); |
34 | TCCR2A |= (1<<WGM21)|(1<<WGM20); |
| 33 | 35 | ||
| 34 | // Timer/Counter 2 Control Register B |
36 | // Timer/Counter 2 Control Register B |
| 35 | 37 | ||
| 36 | // Set clock divider for timer 2 to SYSKLOCK/256 = 20MHz / 256 = 78.128 kHz |
38 | // Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz |
| 37 | // The timer increments from 0x00 to 0xFF with an update rate of 78.128 kHz or 12.8 us |
- | |
| 38 | // hence the timer overflow interrupt frequency is 78.128 kHz / 256 = 305.176 Hz or 3.276 ms |
- | |
| 39 | - | ||
| Line 40... | Line 39... | ||
| 40 | // divider 256 (Bits: CS022 = 1, CS21 = 1, CS20 = 0) |
39 | // The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us |
| 41 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS20)|(1<<WGM22)); |
40 | // hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms |
| Line -... | Line 41... | ||
| - | 41 | ||
| - | 42 | // divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0) |
|
| - | 43 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22)); |
|
| 42 | TCCR2B |= (1<<CS22)|(1<<CS21); |
44 | TCCR2B |= (1<<CS22); |
| 43 | 45 | ||
| 44 | // Initialize the Output Compare Register A used for PWM generation on port PD7. |
46 | // Initialize the Timer/Counter 2 Register |
| 45 | OCR2A = 10; //10 * 12.8us = 1.28 ms high time |
47 | TCNT2 = 0; |
| Line 57... | Line 59... | ||
| 57 | 59 | ||
| 58 | 60 | ||
| 59 | /*****************************************************/ |
61 | /*****************************************************/ |
| 60 | /* Control Servo Position */ |
- | |
| 61 | /*****************************************************/ |
- | |
| 62 | ISR(TIMER2_COMPA_vect) // every OCR2A * 12.8 us (compare match) |
- | |
| Line -... | Line 62... | ||
| - | 62 | /* Control Servo Position */ |
|
| - | 63 | /*****************************************************/ |
|
| - | 64 | ||
| - | 65 | ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A) |
|
| - | 66 | { |
|
| - | 67 | static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match) |
|
| - | 68 | static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl |
|
| - | 69 | static uint16_t ServoState = 40; // cycle down counter for this ISR |
|
| - | 70 | ||
| - | 71 | #define MULTIPLIER 4 |
|
| 63 | { |
72 | |
| 64 | static uint8_t timer = 10; |
73 | |
| - | 74 | ||
| 65 | 75 | switch(ServoState) |
|
| 66 | if(!timer--) |
76 | { |
| - | 77 | case 4: |
|
| 67 | { |
78 | // recalculate new ServoValue |
| 68 | // enable PWM on PD7 in non inverting mode |
79 | ServoValue = 0x0030; // Offset (part 1) |
| 69 | TCCR2A &= ~(0<<COM2A0); |
80 | FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
| 70 | TCCR2A |= (1<<COM2A1); |
81 | ServoValue += FilterServo; // add filtered static offset |
| 71 | 82 | ||
| 72 | ServoValue = FCParam.ServoNickControl; |
83 | if(ParamSet.ServoNickCompInvert & 0x01) |
| 73 | // inverting movment of servo |
- | |
| 74 | if(ParamSet.ServoNickCompInvert & 0x01) |
84 | { // inverting movement of servo |
| 75 | { |
85 | ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
| 76 | ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
- | |
| 77 | } |
86 | } |
| 78 | else // non inverting movement of servo |
87 | else |
| 79 | { |
88 | { // non inverting movement of servo |
| 80 | ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
89 | ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
| 81 | } |
90 | } |
| 82 | 91 | ||
| 83 | // limit servo value to its parameter range definition |
92 | // limit servo value to its parameter range definition |
| 84 | if(ServoValue < ParamSet.ServoNickMin) |
93 | if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) ) |
| - | 94 | { |
|
| 85 | { |
95 | ServoValue = (uint16_t)ParamSet.ServoNickMin * 3; |
| 86 | ServoValue = ParamSet.ServoNickMin; |
96 | } |
| 87 | } |
97 | else |
| 88 | else if(ServoValue > ParamSet.ServoNickMax) |
98 | if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) ) |
| 89 | { |
99 | { |
| - | 100 | ServoValue = (uint16_t)ParamSet.ServoNickMax * 3; |
|
| - | 101 | } |
|
| 90 | ServoValue = ParamSet.ServoNickMax; |
102 | |
| - | 103 | DebugOut.Analog[20] = ServoValue; |
|
| - | 104 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
|
| 91 | } |
105 | if ((ServoValue % 255) < 45) |
| 92 | 106 | { // if prepulse width is to short the execution time of thios isr is longer than the next compare match |
|
| 93 | // update PWM |
107 | // so balance with postpulse width |
| 94 | OCR2A = ServoValue; |
108 | ServoValue += 77; |
| 95 | timer = ParamSet.ServoNickRefresh; |
109 | PostPulse = 0x60 - 77; |
| 96 | } |
110 | } |
| - | 111 | else |
|
| - | 112 | { |
|
| - | 113 | PostPulse = 0x60; |
|
| - | 114 | } |
|
| 97 | else |
115 | // set output compare register to 255 - prepulse width |
| - | 116 | OCR2A = 255 - (ServoValue % 256); |
|
| - | 117 | // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match) |
|
| - | 118 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
|
| - | 119 | ||
| - | 120 | break; |
|
| - | 121 | ||
| - | 122 | case 3: |
|
| - | 123 | case 2: |
|
| - | 124 | case 1: |
|
| 98 | { |
125 | |
| - | 126 | if(ServoValue > 255) // is larger than a full timer 2 cycle |
|
| - | 127 | { |
|
| - | 128 | PORTD |= (1<<PORTD7); // set PD7 to high |
|
| - | 129 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
|
| - | 130 | ServoValue -= 255; // substract full timer cycle |
|
| - | 131 | } |
|
| - | 132 | else // the post pule must be generated |
|
| - | 133 | { |
|
| - | 134 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode |
|
| - | 135 | OCR2A = PostPulse; // Offset Part2 |
|
| - | 136 | ServoState = 1; // jump to ServoState 0 with next ISR call |
|
| - | 137 | } |
|
| - | 138 | break; |
|
| 99 | // disable PWM at PD7 |
139 | |
| - | 140 | case 0: |
|
| - | 141 | ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState |
|
| - | 142 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
|
| - | 143 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
|
| - | 144 | break; |
|
| - | 145 | ||
| 100 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)); |
146 | default: |
| - | 147 | // do nothing |
|
| 101 | // set PD7 to low |
148 | break; |
| - | 149 | } |
|