10,7 → 10,7 |
/* Initialize Timer 2 */ |
/*****************************************************/ |
// The timer 2 is used to generate the PWM at PD7 (J7) |
// to control a camera servo for pitch compensation. |
// to control a camera servo for nick compensation. |
void TIMER2_Init(void) |
{ |
uint8_t sreg = SREG; |
18,7 → 18,7 |
// disable all interrupts before reconfiguration |
cli(); |
|
// set PD7 as output of the PWM for pitch servo |
// set PD7 as output of the PWM for nick servo |
DDRD |=(1<<DDD7); |
PORTD |= (1<<PORTD7); |
|
69,30 → 69,30 |
TCCR2A &= ~(0<<COM2A0); |
TCCR2A |= (1<<COM2A1); |
|
ServoValue = FCParam.ServoPitchControl; |
ServoValue = FCParam.ServoNickControl; |
// inverting movment of servo |
if(ParamSet.ServoPitchCompInvert & 0x01) |
if(ParamSet.ServoNickCompInvert & 0x01) |
{ |
ServoValue += ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512; |
ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
} |
else // non inverting movement of servo |
{ |
ServoValue -= ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512; |
ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512; |
} |
|
// limit servo value to its parameter range definition |
if(ServoValue < ParamSet.ServoPitchMin) |
if(ServoValue < ParamSet.ServoNickMin) |
{ |
ServoValue = ParamSet.ServoPitchMin; |
ServoValue = ParamSet.ServoNickMin; |
} |
else if(ServoValue > ParamSet.ServoPitchMax) |
else if(ServoValue > ParamSet.ServoNickMax) |
{ |
ServoValue = ParamSet.ServoPitchMax; |
ServoValue = ParamSet.ServoNickMax; |
} |
|
// update PWM |
OCR2A = ServoValue; |
timer = ParamSet.ServoPitchRefresh; |
timer = ParamSet.ServoNickRefresh; |
} |
else |
{ |