Subversion Repositories FlightCtrl

Rev

Rev 684 | Rev 687 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 684 Rev 685
Line -... Line 1...
- 
 
 1 
#include <avr/io.h>
- 
 
 2 
#include <avr/interrupt.h>
1 
#include "main.h"
 3 
#include "fc.h"
Line 2... Line -...
2 
 
 - 
 
3 
 
 4 
 
Line 4... Line 5...
4 
int16_t ServoValue = 0;
 5 
volatile int16_t ServoValue = 0;
5 
 
 6 
 
6 
 
 7 
 
7 
/*****************************************************/
 8 
/*****************************************************/
8 
/*              Initialize Timer 2                   */
 9 
/*              Initialize Timer 2                   */
9 
/*****************************************************/
 10 
/*****************************************************/
10 
// The timer 2 is used to generate the PWM at PD7 (J7)
 11 
// The timer 2 is used to generate the PWM at PD7 (J7)
11 
// to control a camera servo for nick compensation
 12 
// to control a camera servo for nick compensation.
Line 12... Line 13...
12 
void TIMER2_Init(void)
 13 
void TIMER2_Init(void)
Line 21... Line 22...
21 
        PORTB |= (1<<PORTD7);
 22 
        PORTB |= (1<<PORTD7);
Line 22... Line 23...
22 
 
 23 
 
Line 23... Line 24...
23 
 
 24 
 
24 
        // Timer/Counter 2 Control Register A
 25 
        // Timer/Counter 2 Control Register A
25 
 
 26 
 
26 
        // Waveform Generation Mode is Fast PWM (Bits WGM22 = 0, WGM21 = 1, WGM20 = 1)
 27 
        // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
27 
    // PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, noninverting PWM (Bits COM2A1 = 1, COM2A0 = 0)
 28 
    // PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, noninverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)
Line 28... Line 29...
28 
    // PD6: Normal port operation, OC2B disconnected, (Bits COM2B1 = 0, COM2B0 = 0)
 29 
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
Line 29... Line 30...
29 
        TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0));
 30 
        TCCR2A &= ~((1<<COM2B1)|(1<<COM2B0)|(1<<COM2A0));
30 
    TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20);
 31 
    TCCR2A |= (1<<COM2A1)|(1<<WGM21)|(1<<WGM20);
31 
 
 32 
 
Line 32... Line 33...
32 
    // Timer/Counter 2 Control Register B
 33 
    // Timer/Counter 2 Control Register B
33 
 
 34 
 
34 
        // set clock devider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz
 35 
        // Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz.
Line 35... Line 36...
35 
        // i.e. the timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz
 36 
        // The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,
36 
        // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz
 37 
        // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz
Line 37... Line 38...
37 
 
 38 
 
38 
    // devider 8 (Bits CS022 = 0, CS21 = 1, CS20 = 0)
 39 
    // divider 8 (Bits: CS022 = 0, CS21 = 1, CS20 = 0)
Line 39... Line 40...
39 
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<WGM22));
 40 
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<WGM22));
40 
    TCCR2B = (TCCR2B & 0xF8)|(0<<CS22)|(1<<CS21)|(0<<CS20);
 41 
    TCCR2B = (TCCR2B & 0xF8)|(0<<CS22)|(1<<CS21)|(0<<CS20);
41 
 
 42 
 
42 
        // initialize the Output Compare Register A used for PWM generation on port PD7
 43 
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
Line 43... Line 44...
43 
        OCR2A = 10;
 44 
        OCR2A = 10;
44 
 
 45 
 
Line 45... Line 46...
45 
        // init Timer/Counter 2 Register
 46 
        // Initialize the Timer/Counter 2 Register
46 
    TCNT2 = 0;
 47 
    TCNT2 = 0;
47 
 
 48 
 
48 
        // Timer/Counter 2 Interrupt Mask Register
 49 
        // Timer/Counter 2 Interrupt Mask Register
49 
        // enable timer output compare match A Interrupt only
 50 
        // Enable timer output compare match A Interrupt only
50 
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
 51 
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
Line 51... Line 52...
51 
        TIMSK2 |= (1<<OCIE2A);
 52 
        TIMSK2 |= (1<<OCIE2A);
52 
 
 53 
 
Line 70... Line 71...
70 
     // inverting movment of servo
 71 
     // inverting movment of servo
71 
     if(EE_Parameter.ServoNickCompInvert & 0x01)
 72 
     if(EE_Parameter.ServoNickCompInvert & 0x01)
72 
     {
 73 
     {
73 
                 ServoValue += ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
 74 
                 ServoValue += ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
74 
         }
 75 
         }
75 
     else // non inverting movment of servo
 76 
     else // non inverting movement of servo
76 
     {
 77 
     {
77 
                 ServoValue -= ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
 78 
                 ServoValue -= ((int32_t) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
78 
         }
 79 
         }
Line 79... Line 80...
79 
 
 80 
 
Line 93... Line 94...
93 
    }
 94 
    }
94 
    else
 95 
    else
95 
    {
 96 
    {
96 
         // disable PWM at PD7
 97 
         // disable PWM at PD7
97 
     TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0));
 98 
     TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0));
98 
     // set PD7 to GND
 99 
     // set PD7 to low
99 
     PORTD &= ~(1<<PORTD7);
 100 
     PORTD &= ~(1<<PORTD7);
100 
    }
 101 
    }
101 
}
 102 
}