Subversion Repositories FlightCtrl

Rev

Rev 935 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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