Subversion Repositories FlightCtrl

Rev

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)
685 killagreg 13
// to control a camera servo for nick compensation.
684 killagreg 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 for the PWM
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
 
685 killagreg 36
        // Set clock divider for timer 2 to SYSKLOCK/8 = 20MHz / 8 = 2.5MHz.
37
        // The timer increments from 0x00 to 0xFF with an update rate of 2.5 MHz,
684 killagreg 38
        // hence the timer overflow interrupt frequency is 2.5 MHz / 256 = 9.765 kHz
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
    {
68
     // enable PWM on PD7 in non inverting mode
69
     TCCR2A = (TCCR2A & 0x3F)|(1<<COM2A1)|(0<<COM2A0);
70
 
71
     ServoValue =  Parameter_ServoNickControl;
72
     // inverting movment of servo
687 killagreg 73
     if(ParamSet.ServoNickCompInvert & 0x01)
684 killagreg 74
     {
687 killagreg 75
                 ServoValue += ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
684 killagreg 76
         }
685 killagreg 77
     else // non inverting movement of servo
684 killagreg 78
     {
687 killagreg 79
                 ServoValue -= ((int32_t) ParamSet.ServoNickComp * (IntegralNick / 128)) / 512;
684 killagreg 80
         }
81
 
82
         // limit servo value to its parameter range definition
687 killagreg 83
     if(ServoValue < ParamSet.ServoNickMin)
684 killagreg 84
     {
687 killagreg 85
                 ServoValue = ParamSet.ServoNickMin;
684 killagreg 86
         }
687 killagreg 87
     else if(ServoValue > ParamSet.ServoNickMax)
684 killagreg 88
     {
687 killagreg 89
                 ServoValue = ParamSet.ServoNickMax;
684 killagreg 90
     }
91
 
92
         // update PWM
93
     OCR2A = ServoValue;
687 killagreg 94
     timer = ParamSet.ServoNickRefresh;
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
}