Rev 689 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 689 | Rev 701 | ||
|---|---|---|---|
| Line 66... | Line 66... | ||
| 66 | if(!timer--) |
66 | if(!timer--) |
| 67 | { |
67 | { |
| 68 | // enable PWM on PD7 in non inverting mode |
68 | // enable PWM on PD7 in non inverting mode |
| 69 | TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0); |
69 | TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0); |
| Line 70... | Line 70... | ||
| 70 | 70 | ||
| 71 | ServoValue = Parameter_ServoNickControl; |
71 | ServoValue = Parameter_ServoPitchControl; |
| 72 | // inverting movment of servo |
72 | // inverting movment of servo |
| 73 | if(ParamSet.ServoNickCompInvert & 0x01) |
73 | if(ParamSet.ServoPitchCompInvert & 0x01) |
| 74 | { |
74 | { |
| 75 | ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
75 | ServoValue += ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512; |
| 76 | } |
76 | } |
| 77 | else // non inverting movement of servo |
77 | else // non inverting movement of servo |
| 78 | { |
78 | { |
| 79 | ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
79 | ServoValue -= ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512; |
| Line 80... | Line 80... | ||
| 80 | } |
80 | } |
| 81 | 81 | ||
| 82 | // limit servo value to its parameter range definition |
82 | // limit servo value to its parameter range definition |
| 83 | if(ServoValue < ParamSet.ServoNickMin) |
83 | if(ServoValue < ParamSet.ServoPitchMin) |
| 84 | { |
84 | { |
| 85 | ServoValue = ParamSet.ServoNickMin; |
85 | ServoValue = ParamSet.ServoPitchMin; |
| 86 | } |
86 | } |
| 87 | else if(ServoValue > ParamSet.ServoNickMax) |
87 | else if(ServoValue > ParamSet.ServoPitchMax) |
| 88 | { |
88 | { |
| Line 89... | Line 89... | ||
| 89 | ServoValue = ParamSet.ServoNickMax; |
89 | ServoValue = ParamSet.ServoPitchMax; |
| 90 | } |
90 | } |
| 91 | 91 | ||
| 92 | // update PWM |
92 | // update PWM |
| 93 | OCR2A = ServoValue; |
93 | OCR2A = ServoValue; |
| 94 | timer = ParamSet.ServoNickRefresh; |
94 | timer = ParamSet.ServoPitchRefresh; |
| 95 | } |
95 | } |
| 96 | else |
96 | else |