Rev 1071 | Rev 1111 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1071 | Rev 1105 | ||
|---|---|---|---|
| Line 7... | Line 7... | ||
| 7 | volatile unsigned int beeptime = 0; |
7 | volatile unsigned int beeptime = 0; |
| 8 | volatile unsigned char SendSPI = 0; |
8 | volatile unsigned char SendSPI = 0; |
| 9 | volatile unsigned int ServoState = 40; |
9 | volatile unsigned int ServoState = 40; |
| Line 10... | Line 10... | ||
| 10 | 10 | ||
| 11 | unsigned int BeepMuster = 0xffff; |
11 | unsigned int BeepMuster = 0xffff; |
| Line 12... | Line 12... | ||
| 12 | unsigned int ServoValue = 0; |
12 | int ServoValue = 0; |
| 13 | 13 | ||
| 14 | enum { |
14 | enum { |
| 15 | STOP = 0, |
15 | STOP = 0, |
| Line 161... | Line 161... | ||
| 161 | if(ServoState == 4) |
161 | if(ServoState == 4) |
| 162 | { |
162 | { |
| 163 | ServoValue = 0x0030; // Offset Part1 |
163 | ServoValue = 0x0030; // Offset Part1 |
| 164 | filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
164 | filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4; |
| 165 | ServoValue += filterServo; |
165 | ServoValue += filterServo; |
| 166 | if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER); |
166 | if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) ); |
| 167 | else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER); |
167 | else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) ); |
| 168 | if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3; |
168 | if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3; |
| 169 | else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3; |
169 | else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3; |
| Line 170... | Line 170... | ||
| 170 | 170 | ||
| 171 | DebugOut.Analog[20] = ServoValue; |
171 | DebugOut.Analog[20] = ServoValue; |