Rev 1077 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1077 | Rev 1078 | ||
---|---|---|---|
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" |
5 | #include "uart.h" |
- | 6 | #include "main.h" |
|
Line 6... | Line 7... | ||
6 | 7 | ||
Line -... | Line 8... | ||
- | 8 | volatile uint16_t ServoValue = 0; |
|
- | 9 | ||
Line 7... | Line 10... | ||
7 | volatile uint16_t ServoValue = 0; |
10 | #define HEF4017R_ON PORTC |= (1<<PORTC6) |
8 | 11 | #define HEF4017R_OFF PORTC &= ~(1<<PORTC6) |
|
9 | 12 | ||
Line 19... | Line 22... | ||
19 | 22 | ||
20 | // disable all interrupts before reconfiguration |
23 | // disable all interrupts before reconfiguration |
Line 21... | Line 24... | ||
21 | cli(); |
24 | cli(); |
22 | 25 | ||
23 | // set PD7 as output of the PWM for nick servo |
26 | // set PD7 as output of the PWM for nick servo |
Line -... | Line 27... | ||
- | 27 | DDRD |= (1<<DDD7); |
|
- | 28 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
|
Line 24... | Line 29... | ||
24 | DDRD |=(1<<DDD7); |
29 | |
Line 25... | Line 30... | ||
25 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
30 | DDRC |= (1<<DDC6); // set PC6 as output (Reset for HEF4017) |
26 | 31 | PORTC &= ~(1<<PORTC6); // set PC6 to low |
|
Line 68... | Line 73... | ||
68 | 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 |
69 | static uint16_t ServoState = 40; // cycle down counter for this ISR |
74 | static uint16_t ServoState = 40; // cycle down counter for this ISR |
Line 70... | Line 75... | ||
70 | 75 | ||
Line 71... | Line -... | ||
71 | #define MULTIPLIER 4 |
- | |
72 | - | ||
73 | 76 | #define MULTIPLIER 4 |
|
74 | 77 | ||
- | 78 | if(BoardRelease < 99) |
|
- | 79 | { |
|
75 | switch(ServoState) |
80 | switch(ServoState) |
76 | { |
81 | { |
77 | case 4: |
82 | case 4: |
78 | // recalculate new ServoValue |
83 | // recalculate new ServoValue |
79 | ServoValue = 0x0030; // Offset (part 1) |
84 | ServoValue = 0x0030; // Offset (part 1) |
80 | 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 |
81 | ServoValue += FilterServo; // add filtered static offset |
86 | ServoValue += FilterServo; // add filtered static offset |
82 | 87 | ||
83 | if(ParamSet.ServoNickCompInvert & 0x01) |
88 | if(ParamSet.ServoNickCompInvert & 0x01) |
84 | { // inverting movement of servo |
89 | { // inverting movement of servo |
85 | ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
90 | ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
86 | } |
91 | } |
87 | else |
92 | else |
88 | { // non inverting movement of servo |
93 | { // non inverting movement of servo |
89 | ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
94 | ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
90 | } |
95 | } |
91 | 96 | ||
92 | // limit servo value to its parameter range definition |
97 | // limit servo value to its parameter range definition |
93 | if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) ) |
98 | if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) ) |
94 | { |
99 | { |
95 | ServoValue = (uint16_t)ParamSet.ServoNickMin * 3; |
100 | ServoValue = (uint16_t)ParamSet.ServoNickMin * 3; |
96 | } |
101 | } |
97 | else |
102 | else |
98 | if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) ) |
103 | if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) ) |
99 | { |
104 | { |
100 | ServoValue = (uint16_t)ParamSet.ServoNickMax * 3; |
105 | ServoValue = (uint16_t)ParamSet.ServoNickMax * 3; |
101 | } |
106 | } |
102 | 107 | ||
103 | DebugOut.Analog[20] = ServoValue; |
108 | DebugOut.Analog[20] = ServoValue; |
104 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
109 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
105 | if ((ServoValue % 255) < 45) |
110 | if ((ServoValue % 255) < 45) |
106 | { // if prepulse width is to short the execution time of thios isr is longer than the next compare match |
111 | { // if prepulse width is to short the execution time of thios isr is longer than the next compare match |
107 | // so balance with postpulse width |
112 | // so balance with postpulse width |
108 | ServoValue += 77; |
113 | ServoValue += 77; |
109 | PostPulse = 0x60 - 77; |
114 | PostPulse = 0x60 - 77; |
110 | } |
115 | } |
111 | else |
116 | else |
112 | { |
117 | { |
113 | PostPulse = 0x60; |
118 | PostPulse = 0x60; |
114 | } |
119 | } |
115 | // set output compare register to 255 - prepulse width |
120 | // set output compare register to 255 - prepulse width |
116 | OCR2A = 255 - (ServoValue % 256); |
121 | OCR2A = 255 - (ServoValue % 256); |
117 | // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match) |
122 | // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match) |
118 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
123 | TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20); |
119 | 124 | ||
120 | break; |
125 | break; |
121 | 126 | ||
122 | case 3: |
127 | case 3: |
123 | case 2: |
128 | case 2: |
124 | case 1: |
129 | case 1: |
125 | 130 | ||
126 | if(ServoValue > 255) // is larger than a full timer 2 cycle |
131 | if(ServoValue > 255) // is larger than a full timer 2 cycle |
127 | { |
132 | { |
128 | PORTD |= (1<<PORTD7); // set PD7 to high |
133 | PORTD |= (1<<PORTD7); // set PD7 to high |
129 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
134 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
130 | ServoValue -= 255; // substract full timer cycle |
135 | ServoValue -= 255; // substract full timer cycle |
131 | } |
136 | } |
132 | else // the post pule must be generated |
137 | else // the post pule must be generated |
133 | { |
138 | { |
134 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode |
139 | TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode |
135 | OCR2A = PostPulse; // Offset Part2 |
140 | 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 |
141 | ServoState = 1; // jump to ServoState 0 with next ISR call |
Line -... | Line 142... | ||
- | 142 | } |
|
- | 143 | break; |
|
- | 144 | ||
- | 145 | case 0: |
|
- | 146 | ServoState = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER; // reload ServoState |
|
- | 147 | PORTD &= ~(1<<PORTD7); // set PD7 to low |
|
143 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
148 | TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A |
144 | break; |
149 | break; |
145 | 150 | ||
146 | default: |
151 | default: |
147 | // do nothing |
152 | // do nothing |
- | 153 | break; |
|
- | 154 | } |
|
- | 155 | ServoState--; |
|
- | 156 | } // EOF BoardRelease < 20 |
|
- | 157 | else // output to HEF4014 |
|
148 | break; |
158 | { |