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 |