Rev 684 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed
| Rev | Author | Line No. | Line |
|---|---|---|---|
| 685 | killagreg | 1 | #include <avr/io.h> |
| 2 | #include <avr/interrupt.h> |
||
| 3 | #include "fc.h" |
||
| 684 | killagreg | 4 | |
| 685 | killagreg | 5 | volatile int16_t ServoValue = 0; |
| 684 | killagreg | 6 | |
| 7 | |||
| 8 | /*****************************************************/ |
||
| 9 | /* Initialize Timer 2 */ |
||
| 10 | /*****************************************************/ |
||
| 11 | // The timer 2 is used to generate the PWM at PD7 (J7) |
||
| 685 | killagreg | 12 | // to control a camera servo for nick compensation. |
| 684 | killagreg | 13 | void TIMER2_Init(void) |
| 14 | { |
||
| 15 | uint8_t sreg = SREG; |
||
| 16 | |||
| 17 | // disable all interrupts before reconfiguration |
||
| 18 | cli(); |
||
| 19 | |||
| 20 | // set PD7 as output for the PWM |
||
| 21 | DDRD |=(1<<DDD7); |
||
| 22 | PORTB |= (1<<PORTD7); |
||
| 23 | |||
| 24 | |||
| 25 | // Timer/Counter 2 Control Register A |
||
| 26 | |||
| 685 | killagreg | 27 | // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1) |
| 28 | // PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, noninverting PWM (Bits: COM2A1 = 1, COM2A0 = 0) |
||
| 29 | // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0) |
||
| 684 | killagreg | 30 | TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0)); |
| 31 | TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20); |
||
| 32 | |||
| 33 | // Timer/Counter 2 Control Register B |
||
| 34 | |||
| 685 | killagreg | 35 | // Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz. |
| 36 | // The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz, |
||
| 684 | killagreg | 37 | // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz |
| 38 | |||
| 685 | killagreg | 39 | // divider 8 (Bits: CS022 = 0, CS21 = 1, CS20 = 0) |
| 684 | killagreg | 40 | TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<WGM22)); |
| 41 | TCCR2B = (TCCR2B & 0xF8)|(0<<CS22)|(1<<CS21)|(0<<CS20); |
||
| 42 | |||
| 685 | killagreg | 43 | // Initialize the Output Compare Register A used for PWM generation on port PD7. |
| 684 | killagreg | 44 | OCR2A = 10; |
| 45 | |||
| 685 | killagreg | 46 | // Initialize the Timer/Counter 2 Register |
| 684 | killagreg | 47 | TCNT2 = 0; |
| 48 | |||
| 49 | // Timer/Counter 2 Interrupt Mask Register |
||
| 685 | killagreg | 50 | // Enable timer output compare match A Interrupt only |
| 684 | killagreg | 51 | TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2)); |
| 52 | TIMSK2 |= (1<<OCIE2A); |
||
| 53 | |||
| 54 | SREG = sreg; |
||
| 55 | } |
||
| 56 | |||
| 57 | |||
| 58 | /*****************************************************/ |
||
| 59 | /* Control Servo Position */ |
||
| 60 | /*****************************************************/ |
||
| 685 | killagreg | 61 | ISR(TIMER2_COMPA_vect) // 9.765 kHz |
| 684 | killagreg | 62 | { |
| 63 | static uint8_t timer = 10; |
||
| 64 | |||
| 65 | if(!timer--) |
||
| 66 | { |
||
| 67 | // enable PWM on PD7 in non inverting mode |
||
| 68 | TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0); |
||
| 69 | |||
| 70 | ServoValue = Parameter_ServoNickControl; |
||
| 71 | // inverting movment of servo |
||
| 72 | if(EE_Parameter.ServoNickCompInvert & 0x01) |
||
| 73 | { |
||
| 74 | ServoValue += ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512; |
||
| 75 | } |
||
| 685 | killagreg | 76 | else // non inverting movement of servo |
| 684 | killagreg | 77 | { |
| 78 | ServoValue -= ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512; |
||
| 79 | } |
||
| 80 | |||
| 81 | // limit servo value to its parameter range definition |
||
| 82 | if(ServoValue < EE_Parameter.ServoNickMin) |
||
| 83 | { |
||
| 84 | ServoValue = EE_Parameter.ServoNickMin; |
||
| 85 | } |
||
| 86 | else if(ServoValue > EE_Parameter.ServoNickMax) |
||
| 87 | { |
||
| 88 | ServoValue = EE_Parameter.ServoNickMax; |
||
| 89 | } |
||
| 90 | |||
| 91 | // update PWM |
||
| 92 | OCR2A = ServoValue; |
||
| 93 | timer = EE_Parameter.ServoNickRefresh; |
||
| 94 | } |
||
| 95 | else |
||
| 96 | { |
||
| 97 | // disable PWM at PD7 |
||
| 98 | TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)); |
||
| 685 | killagreg | 99 | // set PD7 to low |
| 684 | killagreg | 100 | PORTD &= ~(1<<PORTD7); |
| 101 | } |
||
| 102 | } |