Subversion Repositories FlightCtrl

Rev

Rev 886 | Go to most recent revision | 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)
13
// to control a camera servo for pitch compensation.
14
void TIMER2_Init(void)
15
{
16
        uint8_t sreg = SREG;
17
 
18
        // disable all interrupts before reconfiguration
19
        cli();
20
 
21
        // set PD7 as output of the PWM for pitch servo
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
 
72
                 ServoValue =  FCParam.ServoPitchControl;
73
                 // inverting movment of servo
74
                 if(ParamSet.ServoPitchCompInvert & 0x01)
75
                 {
76
                         ServoValue += ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
77
                 }
78
                 else // non inverting movement of servo
79
                 {
80
                         ServoValue -= ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
81
                 }
82
 
83
                 // limit servo value to its parameter range definition
84
                 if(ServoValue < ParamSet.ServoPitchMin)
85
                 {
86
                         ServoValue = ParamSet.ServoPitchMin;
87
                 }
88
                 else if(ServoValue > ParamSet.ServoPitchMax)
89
                 {
90
                         ServoValue = ParamSet.ServoPitchMax;
91
                 }
92
 
93
                 // update PWM
94
                 OCR2A = ServoValue;
95
                 timer = ParamSet.ServoPitchRefresh;
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
}