Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | 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"
687 killagreg 4
#include "eeprom.h"
684 killagreg 5
 
685 killagreg 6
volatile int16_t ServoValue = 0;
684 killagreg 7
 
8
 
9
/*****************************************************/
10
/*              Initialize Timer 2                   */
11
/*****************************************************/
12
// The timer 2 is used to generate the PWM at PD7 (J7)
712 killagreg 13
// to control a camera servo for pitch compensation.
684 killagreg 14
void TIMER2_Init(void)
15
{
16
        uint8_t sreg = SREG;
17
 
18
        // disable all interrupts before reconfiguration
19
        cli();
20
 
766 killagreg 21
        // set PD7 as output of the PWM for pitch servo
684 killagreg 22
        DDRD  |=(1<<DDD7);
23
        PORTB |= (1<<PORTD7);
24
 
25
 
26
        // Timer/Counter 2 Control Register A
27
 
685 killagreg 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, noninverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)
30
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
684 killagreg 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
 
689 killagreg 36
        // Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz
685 killagreg 37
        // The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,
689 killagreg 38
        // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz or 0.1024 ms
684 killagreg 39
 
685 killagreg 40
    // divider 8 (Bits: CS022 = 0, CS21 = 1, CS20 = 0)
684 killagreg 41
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<WGM22));
42
    TCCR2B = (TCCR2B & 0xF8)|(0<<CS22)|(1<<CS21)|(0<<CS20);
43
 
685 killagreg 44
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
684 killagreg 45
        OCR2A = 10;
46
 
685 killagreg 47
        // Initialize the Timer/Counter 2 Register
684 killagreg 48
    TCNT2 = 0;
49
 
50
        // Timer/Counter 2 Interrupt Mask Register
685 killagreg 51
        // Enable timer output compare match A Interrupt only
684 killagreg 52
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
53
        TIMSK2 |= (1<<OCIE2A);
54
 
55
    SREG = sreg;
56
}
57
 
58
 
59
/*****************************************************/
60
/*              Control Servo Position               */
61
/*****************************************************/
685 killagreg 62
ISR(TIMER2_COMPA_vect)  // 9.765 kHz
684 killagreg 63
{
64
  static uint8_t timer = 10;
65
 
66
  if(!timer--)
67
    {
689 killagreg 68
                 // enable PWM on PD7 in non inverting mode
69
                 TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0);
684 killagreg 70
 
703 killagreg 71
                 ServoValue =  FCParam.ServoPitchControl;
689 killagreg 72
                 // inverting movment of servo
701 killagreg 73
                 if(ParamSet.ServoPitchCompInvert & 0x01)
689 killagreg 74
                 {
701 killagreg 75
                         ServoValue += ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
689 killagreg 76
                 }
77
                 else // non inverting movement of servo
78
                 {
701 killagreg 79
                         ServoValue -= ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
689 killagreg 80
                 }
684 killagreg 81
 
689 killagreg 82
                 // limit servo value to its parameter range definition
701 killagreg 83
                 if(ServoValue < ParamSet.ServoPitchMin)
689 killagreg 84
                 {
701 killagreg 85
                         ServoValue = ParamSet.ServoPitchMin;
689 killagreg 86
                 }
701 killagreg 87
                 else if(ServoValue > ParamSet.ServoPitchMax)
689 killagreg 88
                 {
701 killagreg 89
                         ServoValue = ParamSet.ServoPitchMax;
689 killagreg 90
                 }
684 killagreg 91
 
689 killagreg 92
                 // update PWM
93
                 OCR2A = ServoValue;
701 killagreg 94
                 timer = ParamSet.ServoPitchRefresh;
684 killagreg 95
    }
96
    else
97
    {
98
         // disable PWM at PD7
99
     TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0));
685 killagreg 100
     // set PD7 to low
684 killagreg 101
     PORTD &= ~(1<<PORTD7);
102
    }
103
}