Subversion Repositories FlightCtrl

Rev

Rev 723 | Rev 871 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 723 Rev 849
1
#include "main.h"
1
#include "main.h"
2
 
2
 
3
volatile unsigned int CountMilliseconds = 0;
3
volatile unsigned int CountMilliseconds = 0;
4
volatile static unsigned int tim_main;
4
volatile static unsigned int tim_main;
5
volatile unsigned char UpdateMotor = 0;
5
volatile unsigned char UpdateMotor = 0;
6
volatile unsigned int cntKompass = 0;
6
volatile unsigned int cntKompass = 0;
7
volatile unsigned int beeptime = 0;
7
volatile unsigned int beeptime = 0;
8
volatile unsigned char SendSPI = 0;
8
volatile unsigned char SendSPI = 0;
9
 
9
 
10
unsigned int BeepMuster = 0xffff;
10
unsigned int BeepMuster = 0xffff;
11
int ServoValue = 0;
11
int ServoValue = 0;
12
 
12
 
13
enum {
13
enum {
14
  STOP             = 0,
14
  STOP             = 0,
15
  CK               = 1,
15
  CK               = 1,
16
  CK8              = 2,
16
  CK8              = 2,
17
  CK64             = 3,
17
  CK64             = 3,
18
  CK256            = 4,
18
  CK256            = 4,
19
  CK1024           = 5,
19
  CK1024           = 5,
20
  T0_FALLING_EDGE  = 6,
20
  T0_FALLING_EDGE  = 6,
21
  T0_RISING_EDGE   = 7
21
  T0_RISING_EDGE   = 7
22
};
22
};
23
 
23
 
24
 
24
 
25
SIGNAL (SIG_OVERFLOW0)    // 8kHz
25
SIGNAL (SIG_OVERFLOW0)    // 8kHz
26
{
26
{
27
    static unsigned char cnt_1ms = 1,cnt = 0;
27
    static unsigned char cnt_1ms = 1,cnt = 0;
28
    unsigned char pieper_ein = 0;
28
    unsigned char pieper_ein = 0;
29
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
29
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
30
   if(SendSPI) SendSPI--;
30
   if(SendSPI) SendSPI--;
31
   if(!cnt--)
31
   if(!cnt--)
32
    {
32
    {
33
     cnt = 9;
33
     cnt = 9;
34
     cnt_1ms++;
34
     cnt_1ms++;
35
     cnt_1ms %= 2;
35
     cnt_1ms %= 2;
36
     if(!cnt_1ms) UpdateMotor = 1;
36
     if(!cnt_1ms) UpdateMotor = 1;
37
     CountMilliseconds++;
37
     CountMilliseconds++;
38
    }  
38
    }  
39
 
39
 
40
     if(beeptime > 1)
40
     if(beeptime > 1)
41
        {
41
        {
42
        beeptime--;      
42
        beeptime--;      
43
        if(beeptime & BeepMuster)
43
        if(beeptime & BeepMuster)
44
         {
44
         {
45
          pieper_ein = 1;
45
          pieper_ein = 1;
46
         }
46
         }
47
         else pieper_ein = 0;
47
         else pieper_ein = 0;
48
        }
48
        }
49
     else
49
     else
50
      {
50
      {
51
       pieper_ein = 0;
51
       pieper_ein = 0;
52
       BeepMuster = 0xffff;
52
       BeepMuster = 0xffff;
53
      }
53
      }
54
 
54
 
55
 
55
 
56
     if(pieper_ein)
56
     if(pieper_ein)
57
        {
57
        {
58
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
58
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
59
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
59
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
60
        }
60
        }
61
     else  
61
     else  
62
        {
62
        {
63
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
63
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
64
         else                      PORTC &= ~(1<<7);
64
         else                      PORTC &= ~(1<<7);
65
        }
65
        }
66
 
66
 
67
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
67
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
68
 {
68
 {
69
  if(PINC & 0x10)
69
  if(PINC & 0x10)
70
   {
70
   {
71
    cntKompass++;
71
    cntKompass++;
72
   }
72
   }
73
  else
73
  else
74
   {
74
   {
75
    if((cntKompass) && (cntKompass < 4000))
75
    if((cntKompass) && (cntKompass < 362))
76
    {
76
    {
77
     cntKompass += cntKompass / 41;
77
     cntKompass += cntKompass / 41;
78
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
78
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
79
    }
79
    }
80
//     if(cntKompass < 10) cntKompass = 10;
80
//     if(cntKompass < 10) cntKompass = 10;
81
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
81
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
82
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
82
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
83
    cntKompass = 0;
83
    cntKompass = 0;
84
   }
84
   }
85
 }
85
 }
86
}
86
}
87
 
87
 
88
 
88
 
89
void Timer_Init(void)
89
void Timer_Init(void)
90
{
90
{
91
    tim_main = SetDelay(10);
91
    tim_main = SetDelay(10);
92
    TCCR0B = CK8;
92
    TCCR0B = CK8;
93
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
93
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
94
    OCR0A =  0;
94
    OCR0A =  0;
95
    OCR0B = 120;
95
    OCR0B = 120;
96
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
96
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
97
    //OCR1  = 0x00;
97
    //OCR1  = 0x00;
98
 
98
 
99
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
99
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
100
    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
100
    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
101
   
101
   
102
//    TIMSK2 |= _BV(TOIE2);
102
//    TIMSK2 |= _BV(TOIE2);
103
TIMSK2 |= _BV(OCIE2A);
103
TIMSK2 |= _BV(OCIE2A);
104
 
104
 
105
    TIMSK0 |= _BV(TOIE0);
105
    TIMSK0 |= _BV(TOIE0);
106
    OCR2A = 10;
106
    OCR2A = 10;
107
    TCNT2 = 0;
107
    TCNT2 = 0;
108
   
108
   
109
}
109
}
110
 
110
 
111
// -----------------------------------------------------------------------
111
// -----------------------------------------------------------------------
112
 
112
 
113
unsigned int SetDelay (unsigned int t)
113
unsigned int SetDelay (unsigned int t)
114
{
114
{
115
//  TIMSK0 &= ~_BV(TOIE0);
115
//  TIMSK0 &= ~_BV(TOIE0);
116
  return(CountMilliseconds + t + 1);                                            
116
  return(CountMilliseconds + t + 1);                                            
117
//  TIMSK0 |= _BV(TOIE0);
117
//  TIMSK0 |= _BV(TOIE0);
118
}
118
}
119
 
119
 
120
// -----------------------------------------------------------------------
120
// -----------------------------------------------------------------------
121
char CheckDelay(unsigned int t)
121
char CheckDelay(unsigned int t)
122
{
122
{
123
//  TIMSK0 &= ~_BV(TOIE0);
123
//  TIMSK0 &= ~_BV(TOIE0);
124
  return(((t - CountMilliseconds) & 0x8000) >> 9);
124
  return(((t - CountMilliseconds) & 0x8000) >> 9);
125
//  TIMSK0 |= _BV(TOIE0);
125
//  TIMSK0 |= _BV(TOIE0);
126
}
126
}
127
 
127
 
128
// -----------------------------------------------------------------------
128
// -----------------------------------------------------------------------
129
void Delay_ms(unsigned int w)
129
void Delay_ms(unsigned int w)
130
{
130
{
131
 unsigned int akt;
131
 unsigned int akt;
132
 akt = SetDelay(w);
132
 akt = SetDelay(w);
133
 while (!CheckDelay(akt));
133
 while (!CheckDelay(akt));
134
}
134
}
135
 
135
 
136
void Delay_ms_Mess(unsigned int w)
136
void Delay_ms_Mess(unsigned int w)
137
{
137
{
138
 unsigned int akt;
138
 unsigned int akt;
139
 akt = SetDelay(w);
139
 akt = SetDelay(w);
140
 while (!CheckDelay(akt)) ANALOG_ON;
140
 while (!CheckDelay(akt)) ANALOG_ON;
141
}
141
}
142
 
142
 
143
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
143
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
144
//  Servo ansteuern
144
//  Servo ansteuern
145
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
145
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
146
SIGNAL(SIG_OUTPUT_COMPARE2A)
146
SIGNAL(SIG_OUTPUT_COMPARE2A)
147
{
147
{
148
  static unsigned char timer = 10;
148
  static unsigned char timer = 10;
149
 
149
 
150
  if(!timer--)  
150
  if(!timer--)  
151
    {
151
    {
152
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
152
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
153
     ServoValue =  Parameter_ServoNickControl;
153
     ServoValue =  Parameter_ServoNickControl;
154
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
154
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
155
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
155
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
156
     
156
     
157
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
157
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
158
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
158
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
159
 
159
 
160
     OCR2A = ServoValue;// + 75;
160
     OCR2A = ServoValue;// + 75;
161
     timer = EE_Parameter.ServoNickRefresh;
161
     timer = EE_Parameter.ServoNickRefresh;
162
    }
162
    }
163
    else
163
    else
164
    {
164
    {
165
     TCCR2A =3;
165
     TCCR2A =3;
166
     PORTD&=~0x80;
166
     PORTD&=~0x80;
167
    }
167
    }
168
}
168
}
169
 
169