Rev 1104 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1104 | Rev 1107 | ||
---|---|---|---|
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" |
5 | #include "uart.h" |
6 | #include "main.h" |
6 | #include "main.h" |
7 | 7 | ||
8 | volatile int16_t ServoValue = 0; |
8 | volatile int16_t ServoValue = 0; |
9 | 9 | ||
10 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
10 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
11 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
11 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
12 | 12 | ||
13 | 13 | ||
14 | /*****************************************************/ |
14 | /*****************************************************/ |
15 | /* Initialize Timer 2 */ |
15 | /* Initialize Timer 2 */ |
16 | /*****************************************************/ |
16 | /*****************************************************/ |
17 | // The timer 2 is used to generate the PWM at PD7 (J7) |
17 | // The timer 2 is used to generate the PWM at PD7 (J7) |
18 | // to control a camera servo for nick compensation. |
18 | // to control a camera servo for nick compensation. |
19 | void TIMER2_Init(void) |
19 | void TIMER2_Init(void) |
20 | { |
20 | { |
21 | uint8_t sreg = SREG; |
21 | uint8_t sreg = SREG; |
22 | 22 | ||
23 | // disable all interrupts before reconfiguration |
23 | // disable all interrupts before reconfiguration |
24 | cli(); |
24 | cli(); |
25 | 25 | ||
26 | // set PD7 as output of the PWM for nick servo |
26 | // set PD7 as output of the PWM for nick servo |
27 | DDRD |= (1<<DDD7); |
27 | DDRD |= (1<<DDD7); |
28 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
28 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
29 | 29 | ||
30 | DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
30 | DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
31 | PORTC &= ~(1<<PORTC6); // set PC6 to low |
31 | PORTC &= ~(1<<PORTC6); // set PC6 to low |
32 | 32 | ||
33 | // Timer/Counter 2 Control Register A |
33 | // Timer/Counter 2 Control Register A |
34 | 34 | ||
35 | // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1) |
35 | // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1) |
36 | // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
36 | // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0) |
37 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
37 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
38 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
38 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0)); |
39 | TCCR2A |= (1<<WGM21)|(1<<WGM20); |
39 | TCCR2A |= (1<<WGM21)|(1<<WGM20); |
40 | 40 | ||
41 | // Timer/Counter 2 Control Register B |
41 | // Timer/Counter 2 Control Register B |
42 | 42 | ||
43 | // Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz |
43 | // Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz |
44 | // The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us |
44 | // The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us |
45 | // hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms |
45 | // hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms |
46 | 46 | ||
47 | // divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0) |
47 | // divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0) |
48 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22)); |
48 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22)); |
49 | TCCR2B |= (1<<CS22); |
49 | TCCR2B |= (1<<CS22); |
50 | 50 | ||
51 | // Initialize the Timer/Counter 2 Register |
51 | // Initialize the Timer/Counter 2 Register |
52 | TCNT2 = 0; |
52 | TCNT2 = 0; |
53 | 53 | ||
54 | // 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. |
55 | OCR2A = 10; |
55 | OCR2A = 10; |
56 | 56 | ||
57 | // Timer/Counter 2 Interrupt Mask Register |
57 | // Timer/Counter 2 Interrupt Mask Register |
58 | // Enable timer output compare match A Interrupt only |
58 | // Enable timer output compare match A Interrupt only |
59 | TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
59 | TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
60 | TIMSK2 |= (1<<OCIE2A); |
60 | TIMSK2 |= (1<<OCIE2A); |
61 | 61 | ||
62 | SREG = sreg; |
62 | SREG = sreg; |
63 | } |
63 | } |
64 | 64 | ||
65 | 65 | ||
66 | /*****************************************************/ |
66 | /*****************************************************/ |
67 | /* Control Servo Position */ |
67 | /* Control Servo Position */ |
68 | /*****************************************************/ |
68 | /*****************************************************/ |
69 | 69 | ||
70 | ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A) |
70 | ISR(TIMER2_COMPA_vect) // every 256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A) |
71 | { |
71 | { |
72 | static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match) |
72 | static uint8_t PostPulse = 0x80; // value for last pwm cycle in non inverting mode (clear pin on compare match) |
73 | static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl |
73 | static uint16_t FilterServo = 100; // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl |
74 | static uint16_t ServoState = 40; // cycle down counter for this ISR |
74 | static uint16_t ServoState = 40; // cycle down counter for this ISR |
75 | 75 | ||
76 | #define MULTIPLIER 4 |
76 | #define MULTIPLIER 4 |
77 | 77 | ||
78 | if(BoardRelease < 99) |
78 | if(BoardRelease < 99) |
79 | { |
79 | { |
80 | switch(ServoState) |
80 | switch(ServoState) |
81 | { |
81 | { |
82 | case 4: |
82 | case 4: |
83 | // recalculate new ServoValue |
83 | // recalculate new ServoValue |
84 | ServoValue = 0x0030; // Offset (part 1) |
84 | ServoValue = 0x0030; // Offset (part 1) |
85 | DebugOut.Analog[16] = ServoValue; |
- | |
86 | FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
85 | FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
87 | ServoValue += FilterServo; // add filtered static offset |
86 | ServoValue += FilterServo; // add filtered static offset |
88 | - | ||
89 | if(ParamSet.ServoNickCompInvert & 0x01) |
87 | if(ParamSet.ServoNickCompInvert & 0x01) |
90 | { // inverting movement of servo |
88 | { // inverting movement of servo |
91 | ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
89 | ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
92 | } |
90 | } |
93 | else |
91 | else |
94 | { // non inverting movement of servo |
92 | { // non inverting movement of servo |
95 | ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
93 | ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
96 | } |
94 | } |
97 | - | ||
98 | // limit servo value to its parameter range definition |
95 | // limit servo value to its parameter range definition |
99 | if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) ) |
96 | if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) ) |
100 | { |
97 | { |
101 | ServoValue = (int16_t)ParamSet.ServoNickMin * 3; |
98 | ServoValue = (int16_t)ParamSet.ServoNickMin * 3; |
102 | } |
99 | } |
103 | else |
100 | else |
104 | if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) ) |
101 | if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) ) |
105 | { |
102 | { |
106 | ServoValue = (int16_t)ParamSet.ServoNickMax * 3; |
103 | ServoValue = (int16_t)ParamSet.ServoNickMax * 3; |
107 | } |
104 | } |
108 | - | ||
109 | DebugOut.Analog[20] = ServoValue; |
105 | DebugOut.Analog[20] = ServoValue; |
- | 106 | ||
110 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
107 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
111 | if ((ServoValue % 255) < 45) |
108 | if ((ServoValue % 255) < 45) |
112 | { // if prepulse width is to short the execution time of this ISR is longer than the next compare match |
109 | { // if prepulse width is to short the execution time of this ISR is longer than the next compare match |
113 | // so balance with postpulse width |
110 | // so balance with postpulse width |
114 | ServoValue += 77; |
111 | ServoValue += 77; |
115 | PostPulse = 0x60 - 77; |
112 | PostPulse = 0x60 - 77; |
116 | } |
113 | } |
117 | else |
114 | else |
118 | { |
115 | { |
119 | PostPulse = 0x60; |
116 | PostPulse = 0x60; |
120 | } |
117 | } |
121 | // set output compare register to 255 - prepulse width |
118 | // set output compare register to 255 - prepulse width |
122 | OCR2A = 255 - (ServoValue % 256); |
119 | OCR2A = 255 - (ServoValue % 256); |
123 | // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match) |
120 | // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match) |
124 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
121 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
125 | 122 | ||
126 | break; |
123 | break; |
127 | 124 | ||
128 | case 3: |
125 | case 3: |
129 | case 2: |
126 | case 2: |
130 | case 1: |
127 | case 1: |
131 | 128 | ||
132 | if(ServoValue > 255) // is larger than a full timer 2 cycle |
129 | if(ServoValue > 255) // is larger than a full timer 2 cycle |
133 | { |
130 | { |
134 | PORTD |= (1<<PORTD7); // set PD7 to high |
131 | PORTD |= (1<<PORTD7); // set PD7 to high |
135 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
132 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
136 | ServoValue -= 255; // substract full timer cycle |
133 | ServoValue -= 255; // substract full timer cycle |
137 | } |
134 | } |
138 | else // the post pule must be generated |
135 | else // the post pule must be generated |
139 | { |
136 | { |
140 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode |
137 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode |
141 | OCR2A = PostPulse; // Offset Part2 |
138 | OCR2A = PostPulse; // Offset Part2 |
142 | ServoState = 1; // jump to ServoState 0 with next ISR call |
139 | ServoState = 1; // jump to ServoState 0 with next ISR call |
143 | } |
140 | } |
144 | break; |
141 | break; |
145 | 142 | ||
146 | case 0: |
143 | case 0: |
147 | ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState |
144 | ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState |
148 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
145 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
149 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
146 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
150 | break; |
147 | break; |
151 | 148 | ||
152 | default: |
149 | default: |
153 | // do nothing |
150 | // do nothing |
154 | break; |
151 | break; |
155 | } |
152 | } |
156 | ServoState--; |
153 | ServoState--; |
157 | } // EOF BoardRelease < 20 |
154 | } // EOF BoardRelease < 20 |
158 | else // output to HEF4014 |
155 | else // output to HEF4014 |
159 | { |
156 | { |
160 | // code for HEF4014 output must be placed here |
157 | // code for HEF4014 output must be placed here |
161 | } |
158 | } |
162 | } |
159 | } |
163 | 160 | ||
164 | 161 |