Subversion Repositories FlightCtrl

Rev

Details | 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"
936 killagreg 5
#include "uart.h"
1078 killagreg 6
#include "main.h"
886 killagreg 7
 
1104 killagreg 8
volatile int16_t ServoValue = 0;
886 killagreg 9
 
1078 killagreg 10
#define HEF4017R_ON     PORTC |=  (1<<PORTC6)
11
#define HEF4017R_OFF    PORTC &= ~(1<<PORTC6)
886 killagreg 12
 
936 killagreg 13
 
886 killagreg 14
/*****************************************************/
15
/*              Initialize Timer 2                   */
16
/*****************************************************/
17
// The timer 2 is used to generate the PWM at PD7 (J7)
911 killagreg 18
// to control a camera servo for nick compensation.
886 killagreg 19
void TIMER2_Init(void)
20
{
21
        uint8_t sreg = SREG;
22
 
23
        // disable all interrupts before reconfiguration
24
        cli();
25
 
911 killagreg 26
        // set PD7 as output of the PWM for nick servo
1078 killagreg 27
        DDRD  |= (1<<DDD7);
936 killagreg 28
        PORTD &= ~(1<<PORTD7);  // set PD7 to low
886 killagreg 29
 
1078 killagreg 30
        DDRC  |= (1<<DDC6);     // set PC6 as output (Reset for HEF4017)
31
        PORTC &= ~(1<<PORTC6);  // set PC6 to low
886 killagreg 32
 
33
        // Timer/Counter 2 Control Register A
34
 
35
        // Waveform Generation Mode is Fast PWM (Bits: WGM22 = 0, WGM21 = 1, WGM20 = 1)
936 killagreg 36
    // PD7: Normal port operation, OC2A disconnected, (Bits: COM2A1 = 0, COM2A0 = 0)
886 killagreg 37
    // PD6: Normal port operation, OC2B disconnected, (Bits: COM2B1 = 0, COM2B0 = 0)
936 killagreg 38
        TCCR2A &= ~((1<<COM2A1)|(1<<COM2A0)|(1<<COM2B1)|(1<<COM2B0));
39
    TCCR2A |= (1<<WGM21)|(1<<WGM20);
886 killagreg 40
 
41
    // Timer/Counter 2 Control Register B
42
 
936 killagreg 43
        // Set clock divider for timer 2 to SYSKLOCK/64 = 20MHz / 64 = 312.5 kHz
44
        // The timer increments from 0x00 to 0xFF with an update rate of 312.5 kHz or 3.2 us
45
        // hence the timer overflow interrupt frequency is 312.5 kHz / 256 = 1220.7 Hz or 0.8192 ms
886 killagreg 46
 
936 killagreg 47
    // divider 64 (Bits: CS022 = 1, CS21 = 0, CS20 = 0)
48
        TCCR2B &= ~((1<<FOC2A)|(1<<FOC2B)|(1<<CS21)|(1<<CS20)|(1<<WGM22));
49
    TCCR2B |= (1<<CS22);
886 killagreg 50
 
51
        // Initialize the Timer/Counter 2 Register
52
    TCNT2 = 0;
53
 
936 killagreg 54
        // Initialize the Output Compare Register A used for PWM generation on port PD7.
55
        OCR2A = 10;
56
 
886 killagreg 57
        // Timer/Counter 2 Interrupt Mask Register
58
        // Enable timer output compare match A Interrupt only
59
        TIMSK2 &= ~((1<<OCIE2B)|(1<<TOIE2));
60
        TIMSK2 |= (1<<OCIE2A);
61
 
62
    SREG = sreg;
63
}
64
 
65
 
66
/*****************************************************/
67
/*              Control Servo Position               */
68
/*****************************************************/
936 killagreg 69
 
70
ISR(TIMER2_COMPA_vect)  // every  256 * 3.2 us = 0.819 us ( on compare match of TCNT2 and OC2A)
886 killagreg 71
{
936 killagreg 72
        static  uint8_t PostPulse = 0x80;       // value for last pwm cycle in non inverting mode (clear pin on compare match)
73
        static uint16_t FilterServo = 100;      // initial value, after some iterations it becomes the average value of 2 * FCParam.ServoNickControl
74
        static uint16_t ServoState = 40;        // cycle down counter for this ISR
886 killagreg 75
 
936 killagreg 76
        #define MULTIPLIER 4
886 killagreg 77
 
1078 killagreg 78
        if(BoardRelease < 99)
79
        {
80
                switch(ServoState)
81
                {
82
                        case 4:
83
                                // recalculate new ServoValue
84
                                ServoValue = 0x0030; // Offset (part 1)
85
                                FilterServo = (3 * FilterServo + (uint16_t)FCParam.ServoNickControl * 2) / 4; // lowpass static offset
86
                                ServoValue += FilterServo; // add filtered static offset
87
                                if(ParamSet.ServoNickCompInvert & 0x01)
88
                                {       // inverting movement of servo
1104 killagreg 89
                                        ServoValue += (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
1078 killagreg 90
                                }
91
                                else
92
                                {       // non inverting movement of servo
1104 killagreg 93
                                        ServoValue -= (int16_t)( ( (int32_t)ParamSet.ServoNickComp * (IntegralNick / 128L ) ) / (512L/MULTIPLIER) );
1078 killagreg 94
                                }
95
                                // limit servo value to its parameter range definition
1104 killagreg 96
                                if(ServoValue < ((int16_t)ParamSet.ServoNickMin * 3) )
1078 killagreg 97
                                {
1104 killagreg 98
                                        ServoValue = (int16_t)ParamSet.ServoNickMin * 3;
1078 killagreg 99
                                }
100
                                else
1104 killagreg 101
                                if(ServoValue > ((int16_t)ParamSet.ServoNickMax * 3) )
1078 killagreg 102
                                {
1104 killagreg 103
                                        ServoValue = (int16_t)ParamSet.ServoNickMax * 3;
1078 killagreg 104
                                }
1107 killagreg 105
                                DebugOut.Analog[20] = ServoValue;
936 killagreg 106
 
1078 killagreg 107
                                // determine prepulse width (remaining part of ServoValue/Timer Cycle)
108
                                if ((ServoValue % 255) < 45)
1104 killagreg 109
                                {       // if prepulse width is to short the execution time of this ISR is longer than the next compare match
1078 killagreg 110
                                        // so balance with postpulse width
111
                                        ServoValue += 77;
112
                                        PostPulse = 0x60 - 77;
113
                                }
114
                                else
115
                                {
116
                                        PostPulse = 0x60;
117
                                }
118
                                // set output compare register to 255 - prepulse width
119
                                OCR2A = 255 - (ServoValue % 256);
120
                                // connect OC2A in inverting mode (Clear pin on overflow, Set pin on compare match)
121
                                TCCR2A=(1<<COM2A1)|(1<<COM2A0)|(1<<WGM21)|(1<<WGM20);
936 killagreg 122
 
1078 killagreg 123
                                break;
936 killagreg 124
 
1078 killagreg 125
                        case 3:
126
                        case 2:
127
                        case 1:
936 killagreg 128
 
1078 killagreg 129
                                if(ServoValue > 255)        // is larger than a full timer 2 cycle
130
                                {
131
                                        PORTD |= (1<<PORTD7);                   // set PD7 to high
132
                                        TCCR2A = (1<<WGM21)|(1<<WGM20); // disconnect OC2A
133
                                        ServoValue -= 255;              // substract full timer cycle
134
                                }
135
                                else // the post pule must be generated
136
                                {
137
                                        TCCR2A=(1<<COM2A1)|(0<<COM2A0)|(1<<WGM21)|(1<<WGM20); // connect OC2A in non inverting mode
138
                                        OCR2A = PostPulse; // Offset Part2
139
                                        ServoState = 1;    // jump to ServoState 0 with next ISR call
140
                                }
936 killagreg 141
                        break;
142
 
1078 killagreg 143
                        case 0:
144
                                ServoState  = (uint16_t) ParamSet.ServoNickRefresh * MULTIPLIER;        // reload ServoState
145
                                PORTD &= ~(1<<PORTD7);                                                                                          // set PD7 to low
146
                                TCCR2A = (1<<WGM21)|(1<<WGM20);                                                         // disconnect OC2A
147
                                break;
936 killagreg 148
 
1078 killagreg 149
                        default:
150
                                // do nothing
151
                                break;
152
                }
153
        ServoState--;
154
        } // EOF BoardRelease < 20
155
        else // output to HEF4014
156
        {
157
                // code for HEF4014 output must be placed here
936 killagreg 158
        }
886 killagreg 159
}
936 killagreg 160