Rev 1104 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1104 | Rev 1107 | ||
|---|---|---|---|
| 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; |
- | |
| 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; |