Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
425 Nick666 1
#include "main.h"
2
 
3
volatile unsigned int CountMilliseconds = 0;
4
volatile static unsigned int tim_main;
5
volatile unsigned char UpdateMotor = 0;
6
volatile unsigned int beeptime = 0;
7
volatile unsigned int cntKompass = 800;
8
int ServoValue = 0;
9
 
10
enum {
11
  STOP             = 0,
12
  CK               = 1,
13
  CK8              = 2,
14
  CK64             = 3,
15
  CK256            = 4,
16
  CK1024           = 5,
17
  T0_FALLING_EDGE  = 6,
18
  T0_RISING_EDGE   = 7
19
};
20
 
21
 
22
SIGNAL (SIG_OVERFLOW0)    // 9,8kHz
23
{
24
    static unsigned char cnt_1ms = 1,cnt = 0;
25
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
26
 
27
   if(!cnt--)
28
    {
29
     cnt = 10;
30
     cnt_1ms++;
31
     cnt_1ms %= 2;
32
     if(!cnt_1ms) UpdateMotor = 1;
33
     CountMilliseconds++;
34
     if(Timeout) Timeout--;
35
     }  
36
 
37
     if(beeptime > 1)
38
        {
39
        beeptime--;
40
        PORTD |= (1<<PD2);
41
        }
42
     else  
43
        PORTD &= ~(1<<PD2);
44
 
45
        if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
46
        {
47
                timer0_MM3();           // Kompass auslesen
48
 
49
                if (!cntKompass--)              // Aufruf mit 10 Hz
50
                {
51
                        int heading;
52
                        heading = heading_MM3();
53
                        KompassValue = (KompassValue * 3 + heading) / 4;                // Filtern
54
                        KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
55
                        cntKompass = 980;
56
                }
57
        }
58
}
59
 
60
 
61
void Timer_Init(void)
62
{
63
    tim_main = SetDelay(10);
64
    TCCR0B = CK8;
65
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
66
    OCR0A =  0;
67
    OCR0B = 120;
68
    //TCNT0 = -TIMER_RELOAD_VALUE;  // reload
69
    //OCR1  = 0x00;
70
 
71
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
72
    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
73
 
74
//    TIMSK2 |= _BV(TOIE2);
75
TIMSK2 |= _BV(OCIE2A);
76
 
77
    TIMSK0 |= _BV(TOIE0);
78
    OCR2A = 10;
79
    TCNT2 = 0;
80
 
81
}
82
 
83
// -----------------------------------------------------------------------
84
 
85
unsigned int SetDelay (unsigned int t)
86
{
87
//  TIMSK0 &= ~_BV(TOIE0);
88
  return(CountMilliseconds + t + 1);                                            
89
//  TIMSK0 |= _BV(TOIE0);
90
}
91
 
92
// -----------------------------------------------------------------------
93
char CheckDelay(unsigned int t)
94
{
95
//  TIMSK0 &= ~_BV(TOIE0);
96
  return(((t - CountMilliseconds) & 0x8000) >> 9);
97
//  TIMSK0 |= _BV(TOIE0);
98
}
99
 
100
// -----------------------------------------------------------------------
101
void Delay_ms(unsigned int w)
102
{
103
 unsigned int akt;
104
 akt = SetDelay(w);
105
 while (!CheckDelay(akt));
106
}
107
 
108
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
109
//  Servo ansteuern
110
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
111
SIGNAL(SIG_OUTPUT_COMPARE2A)
112
{
113
  static unsigned char timer = 10;
114
 
115
  if(!timer--)  
116
    {
117
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
118
     ServoValue =  Parameter_ServoNickControl;
119
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
120
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
121
 
122
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
123
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
124
 
125
     //DebugOut.Analog[10] = ServoValue;     
126
     OCR2A = ServoValue;// + 75;
127
     timer = EE_Parameter.ServoNickRefresh;
128
    }
129
    else
130
    {
131
     TCCR2A =3;
132
     PORTD&=~0x80;
133
    }
134
}