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