Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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