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