Rev 935 | Go to most recent revision | Only display areas with differences | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 935 | Rev 936 | ||
|---|---|---|---|
| 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; |
|
| 5 | - | ||
| 6 | volatile int16_t ServoValue = 0; |
8 | |
| 7 | 9 | ||
| 8 | 10 | ||
| 9 | /*****************************************************/ |
11 | /*****************************************************/ |
| 10 | /* Initialize Timer 2 */ |
12 | /* Initialize Timer 2 */ |
| 11 | /*****************************************************/ |
13 | /*****************************************************/ |
| 12 | // The timer 2 is used to generate the PWM at PD7 (J7) |
14 | // The timer 2 is used to generate the PWM at PD7 (J7) |
| 13 | // to control a camera servo for nick compensation. |
15 | // to control a camera servo for nick compensation. |
| 14 | void TIMER2_Init(void) |
16 | void TIMER2_Init(void) |
| 15 | { |
17 | { |
| 16 | uint8_t sreg = SREG; |
18 | uint8_t sreg = SREG; |
| 17 | 19 | ||
| 18 | // disable all interrupts before reconfiguration |
20 | // disable all interrupts before reconfiguration |
| 19 | cli(); |
21 | cli(); |
| 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); |
| 23 | PORTD |= (1<<PORTD7); |
25 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
| 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) |
| 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) |
| 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 |
39 | // The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us |
| 38 | // hence the timer overflow interrupt frequency is 78.128 kHz / 256 = 305.176 Hz or 3.276 ms |
40 | // hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms |
| 39 | 41 | ||
| 40 | // divider 256 (Bits: CS022 = 1, CS21 = 1, CS20 = 0) |
42 | // divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0) |
| 41 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS20)|(1<<WGM22)); |
43 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22)); |
| 42 | TCCR2B |= (1<<CS22)|(1<<CS21); |
- | |
| 43 | - | ||
| 44 | // Initialize the Output Compare Register A used for PWM generation on port PD7. |
- | |
| 45 | OCR2A = 10; //10 * 12.8us = 1.28 ms high time |
44 | TCCR2B |= (1<<CS22); |
| 46 | 45 | ||
| 47 | // Initialize the Timer/Counter 2 Register |
46 | // Initialize the Timer/Counter 2 Register |
| 48 | TCNT2 = 0; |
47 | TCNT2 = 0; |
| - | 48 | ||
| - | 49 | // Initialize the Output Compare Register A used for PWM generation on port PD7. |
|
| - | 50 | OCR2A = 10; |
|
| 49 | 51 | ||
| 50 | // Timer/Counter 2 Interrupt Mask Register |
52 | // Timer/Counter 2 Interrupt Mask Register |
| 51 | // Enable timer output compare match A Interrupt only |
53 | // Enable timer output compare match A Interrupt only |
| 52 | TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
54 | TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
| 53 | TIMSK2 |= (1<<OCIE2A); |
55 | TIMSK2 |= (1<<OCIE2A); |
| 54 | 56 | ||
| 55 | SREG = sreg; |
57 | SREG = sreg; |
| 56 | } |
58 | } |
| 57 | 59 | ||
| 58 | 60 | ||
| 59 | /*****************************************************/ |
61 | /*****************************************************/ |
| 60 | /* Control Servo Position */ |
62 | /* Control Servo Position */ |
| 61 | /*****************************************************/ |
63 | /*****************************************************/ |
| - | 64 | ||
| 62 | ISR(TIMER2_COMPA_vect) // every OCR2A * 12.8 us (compare match) |
65 | ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A) |
| 63 | { |
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 |
|
| 64 | static uint8_t timer = 10; |
69 | static uint16_t ServoState = 40; // cycle down counter for this ISR |
| - | 70 | ||
| - | 71 | #define MULTIPLIER 4 |
|
| - | 72 | ||
| - | 73 | ||
| 65 | 74 | ||
| 66 | if(!timer--) |
75 | switch(ServoState) |
| - | 76 | { |
|
| 67 | { |
77 | case 4: |
| 68 | // enable PWM on PD7 in non inverting mode |
78 | // recalculate new ServoValue |
| - | 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); |
- | |
| 71 | - | ||
| 72 | ServoValue = FCParam.ServoNickControl; |
81 | ServoValue += FilterServo; // add filtered static offset |
| 73 | // inverting movment of servo |
82 | |
| 74 | if(ParamSet.ServoNickCompInvert & 0x01) |
83 | if(ParamSet.ServoNickCompInvert & 0x01) |
| 75 | { |
84 | { // inverting movement of servo |
| - | 85 | ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
|
| 76 | ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
86 | } |
| 77 | } |
- | |
| 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) ) |
| 85 | { |
94 | { |
| 86 | ServoValue = ParamSet.ServoNickMin; |
95 | ServoValue = (uint16_t)ParamSet.ServoNickMin * 3; |
| 87 | } |
96 | } |
| - | 97 | else |
|
| 88 | else if(ServoValue > ParamSet.ServoNickMax) |
98 | if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) ) |
| 89 | { |
99 | { |
| 90 | ServoValue = ParamSet.ServoNickMax; |
100 | ServoValue = (uint16_t)ParamSet.ServoNickMax * 3; |
| 91 | } |
101 | } |
| - | 102 | ||
| - | 103 | DebugOut.Analog[20] = ServoValue; |
|
| 92 | 104 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
|
| - | 105 | if ((ServoValue % 255) < 45) |
|
| - | 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 | } |
| 97 | else |
111 | else |
| 98 | { |
112 | { |
| 99 | // disable PWM at PD7 |
113 | PostPulse = 0x60; |
| - | 114 | } |
|
| - | 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) |
|
| 100 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)); |
118 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
| - | 119 | ||
| - | 120 | break; |
|
| - | 121 | ||
| - | 122 | case 3: |
|
| - | 123 | case 2: |
|
| 101 | // set PD7 to low |
124 | case 1: |
| - | 125 | ||
| - | 126 | if(ServoValue > 255) // is larger than a full timer 2 cycle |
|
| - | 127 | { |
|
| 102 | PORTD &= ~(1<<PORTD7); |
128 | PORTD |= (1<<PORTD7); // set PD7 to high |
| - | 129 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
|
| - | 130 | ServoValue -= 255; // substract full timer cycle |
|
| 103 | } |
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; |
|
| - | 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 | ||
| - | 146 | default: |
|
| - | 147 | // do nothing |
|
| - | 148 | break; |
|
| - | 149 | } |
|
| - | 150 | ServoState--; |
|
| 104 | } |
151 | } |
| - | 152 | ||
| 105 | 153 | ||