Rev 1078 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1078 | Rev 1104 | ||
|---|---|---|---|
| Line 3... | Line 3... | ||
| 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" |
| Line 7... | Line 7... | ||
| 7 | 7 | ||
| Line 8... | Line 8... | ||
| 8 | volatile uint16_t ServoValue = 0; |
8 | volatile int16_t ServoValue = 0; |
| 9 | 9 | ||
| Line 80... | Line 80... | ||
| 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; |
|
| 85 | FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
86 | FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset |
| 86 | ServoValue += FilterServo; // add filtered static offset |
87 | ServoValue += FilterServo; // add filtered static offset |
| Line 87... | Line 88... | ||
| 87 | 88 | ||
| 88 | if(ParamSet.ServoNickCompInvert & 0x01) |
89 | if(ParamSet.ServoNickCompInvert & 0x01) |
| 89 | { // inverting movement of servo |
90 | { // inverting movement of servo |
| 90 | ServoValue += ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
91 | ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
| 91 | } |
92 | } |
| 92 | else |
93 | else |
| 93 | { // non inverting movement of servo |
94 | { // non inverting movement of servo |
| 94 | ServoValue -= ((int32_t) ((int32_t)ParamSet.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
95 | ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) ); |
| Line 95... | Line 96... | ||
| 95 | } |
96 | } |
| 96 | 97 | ||
| 97 | // limit servo value to its parameter range definition |
98 | // limit servo value to its parameter range definition |
| 98 | if(ServoValue < ((uint16_t)ParamSet.ServoNickMin * 3) ) |
99 | if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) ) |
| 99 | { |
100 | { |
| 100 | ServoValue = (uint16_t)ParamSet.ServoNickMin * 3; |
101 | ServoValue = (int16_t)ParamSet.ServoNickMin * 3; |
| 101 | } |
102 | } |
| 102 | else |
103 | else |
| 103 | if(ServoValue > ((uint16_t)ParamSet.ServoNickMax * 3) ) |
104 | if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) ) |
| 104 | { |
105 | { |
| Line 105... | Line 106... | ||
| 105 | ServoValue = (uint16_t)ParamSet.ServoNickMax * 3; |
106 | ServoValue = (int16_t)ParamSet.ServoNickMax * 3; |
| 106 | } |
107 | } |
| 107 | 108 | ||
| 108 | DebugOut.Analog[20] = ServoValue; |
109 | DebugOut.Analog[20] = ServoValue; |
| 109 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
110 | // determine prepulse width (remaining part of ServoValue/Timer Cycle) |
| 110 | if ((ServoValue % 255) < 45) |
111 | if ((ServoValue % 255) < 45) |
| 111 | { // if prepulse width is to short the execution time of thios isr is longer than the next compare match |
112 | { // if prepulse width is to short the execution time of this ISR is longer than the next compare match |
| 112 | // so balance with postpulse width |
113 | // so balance with postpulse width |
| 113 | ServoValue += 77; |
114 | ServoValue += 77; |