Subversion Repositories FlightCtrl

Rev

Rev 1086 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

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