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);
783 killagreg 23
        PORTD |= (1<<PORTD7);
684 killagreg 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)
783 killagreg 29
    // PD7: Clear OC2B on Compare Match, set OC2B at BOTTOM, non inverting PWM (Bits: COM2A1 = 1, COM2A0 = 0)
685 killagreg 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
 
783 killagreg 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
684 killagreg 39
 
783 killagreg 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);
684 killagreg 43
 
685 killagreg 44
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
783 killagreg 45
        OCR2A = 10; //10 * 12.8us = 1.28 ms high time
684 killagreg 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
/*****************************************************/
783 killagreg 62
ISR(TIMER2_COMPA_vect)  // every  OCR2A * 12.8s (compare match)
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
783 killagreg 69
                 TCCR2A &= ~(0<<COM2A0);
70
                 TCCR2A |= (1<<COM2A1);
684 killagreg 71
 
703 killagreg 72
                 ServoValue =  FCParam.ServoPitchControl;
689 killagreg 73
                 // inverting movment of servo
701 killagreg 74
                 if(ParamSet.ServoPitchCompInvert & 0x01)
689 killagreg 75
                 {
701 killagreg 76
                         ServoValue += ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
689 killagreg 77
                 }
78
                 else // non inverting movement of servo
79
                 {
701 killagreg 80
                         ServoValue -= ((int32_t) ParamSet.ServoPitchComp * (IntegralPitch / 128)) / 512;
689 killagreg 81
                 }
684 killagreg 82
 
689 killagreg 83
                 // limit servo value to its parameter range definition
701 killagreg 84
                 if(ServoValue < ParamSet.ServoPitchMin)
689 killagreg 85
                 {
701 killagreg 86
                         ServoValue = ParamSet.ServoPitchMin;
689 killagreg 87
                 }
701 killagreg 88
                 else if(ServoValue > ParamSet.ServoPitchMax)
689 killagreg 89
                 {
701 killagreg 90
                         ServoValue = ParamSet.ServoPitchMax;
689 killagreg 91
                 }
684 killagreg 92
 
689 killagreg 93
                 // update PWM
94
                 OCR2A = ServoValue;
701 killagreg 95
                 timer = ParamSet.ServoPitchRefresh;
684 killagreg 96
    }
97
    else
98
    {
99
         // disable PWM at PD7
100
     TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0));
685 killagreg 101
     // set PD7 to low
684 killagreg 102
     PORTD &= ~(1<<PORTD7);
103
    }
104
}