| 68,30 → 68,30 |
|---|
| // enable PWM on PD7 in non inverting mode |
| TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0); |
| |
| ServoValue = Parameter_ServoNickControl; |
| ServoValue = Parameter_ServoPitchControl; |
| // inverting movment of servo |
| if(ParamSet.ServoNickCompInvert & 0x01) |
| if(ParamSet.ServoPitchCompInvert & 0x01) |
| { |
| ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
| ServoValue += ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512; |
| } |
| else // non inverting movement of servo |
| { |
| ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
| ServoValue -= ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512; |
| } |
| |
| // limit servo value to its parameter range definition |
| if(ServoValue < ParamSet.ServoNickMin) |
| if(ServoValue < ParamSet.ServoPitchMin) |
| { |
| ServoValue = ParamSet.ServoNickMin; |
| ServoValue = ParamSet.ServoPitchMin; |
| } |
| else if(ServoValue > ParamSet.ServoNickMax) |
| else if(ServoValue > ParamSet.ServoPitchMax) |
| { |
| ServoValue = ParamSet.ServoNickMax; |
| ServoValue = ParamSet.ServoPitchMax; |
| } |
| |
| // update PWM |
| OCR2A = ServoValue; |
| timer = ParamSet.ServoNickRefresh; |
| timer = ParamSet.ServoPitchRefresh; |
| } |
| else |
| { |