Subversion Repositories FlightCtrl

Rev

Rev 319 | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
308 osiair 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 = 0;
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)    // 8kHz
23
{
24
    static unsigned char cnt_1ms = 1,cnt = 0;
25
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
26
 
27
   if(!cnt--)
28
    {
29
     cnt = 9;
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
                MM3_timer0();           // Kompass auslesen
48
 
49
                if (!cntKompass--)              // Aufruf mit 25 Hz
50
                {
309 osiair 51
 
310 osiair 52
                if (MM3_heading() > 0) { KompassValue = 360-MM3_heading();}
53
                if (MM3_heading() < 0 ) { KompassValue = MM3_heading()*-1;}
309 osiair 54
 
319 osiair 55
                        cntKompass = 640;
308 osiair 56
                }
57
        }
58
 
59
 
60
}
61
 
62
 
63
void Timer_Init(void)
64
{
65
    tim_main = SetDelay(10);
66
    TCCR0B = CK8;
67
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
68
    OCR0A =  0;
69
    OCR0B = 120;
70
    TCNT0 = -TIMER_RELOAD_VALUE;  // reload
71
    //OCR1  = 0x00;
72
 
73
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
74
    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
75
 
76
//    TIMSK2 |= _BV(TOIE2);
77
TIMSK2 |= _BV(OCIE2A);
78
 
79
    TIMSK0 |= _BV(TOIE0);
80
    OCR2A = 10;
81
    TCNT2 = 0;
82
 
83
}
84
 
85
// -----------------------------------------------------------------------
86
 
87
unsigned int SetDelay (unsigned int t)
88
{
89
//  TIMSK0 &= ~_BV(TOIE0);
90
  return(CountMilliseconds + t + 1);                                            
91
//  TIMSK0 |= _BV(TOIE0);
92
}
93
 
94
// -----------------------------------------------------------------------
95
char CheckDelay(unsigned int t)
96
{
97
//  TIMSK0 &= ~_BV(TOIE0);
98
  return(((t - CountMilliseconds) & 0x8000) >> 9);
99
//  TIMSK0 |= _BV(TOIE0);
100
}
101
 
102
// -----------------------------------------------------------------------
103
void Delay_ms(unsigned int w)
104
{
105
 unsigned int akt;
106
 akt = SetDelay(w);
107
 while (!CheckDelay(akt));
108
}
109
 
110
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
111
//  Servo ansteuern
112
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
113
SIGNAL(SIG_OUTPUT_COMPARE2A)
114
{
115
  static unsigned char timer = 10;
116
 
117
  if(!timer--)  
118
    {
119
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
120
     ServoValue =  Parameter_ServoNickControl;
121
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
122
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
123
 
124
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
125
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
126
 
127
     //DebugOut.Analog[10] = ServoValue;     
128
     OCR2A = ServoValue;// + 75;
129
     timer = EE_Parameter.ServoNickRefresh;
130
    }
131
    else
132
    {
133
     TCCR2A =3;
134
     PORTD&=~0x80;
135
    }
136
}