Rev 1077 | Go to most recent revision | 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 | { |