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 |
{ |