Subversion Repositories FlightCtrl

Rev

Rev 1071 | Rev 1111 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1071 Rev 1105
Line 7... Line 7...
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 
volatile unsigned int ServoState = 40;
 9 
volatile unsigned int ServoState = 40;
Line 10... Line 10...
10 
 
 10 
 
11 
unsigned int BeepMuster = 0xffff;
 11 
unsigned int BeepMuster = 0xffff;
Line 12... Line 12...
12 
unsigned int ServoValue = 0;
 12 
int ServoValue = 0;
13 
 
 13 
 
14 
enum {
 14 
enum {
15 
  STOP             = 0,
 15 
  STOP             = 0,
Line 29... Line 29...
29 
    unsigned char pieper_ein = 0;
 29 
    unsigned char pieper_ein = 0;
30 
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
 30 
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
31 
   if(SendSPI) SendSPI--;
 31 
   if(SendSPI) SendSPI--;
32 
   if(!cnt--)
 32 
   if(!cnt--)
33 
    {
 33 
    {
34 
     cnt = 9;
 34 
     cnt = 9;
35 
     cnt_1ms++;
 35 
     cnt_1ms++;
36 
     cnt_1ms %= 2;
 36 
     cnt_1ms %= 2;
37 
     if(!cnt_1ms) UpdateMotor = 1;
 37 
     if(!cnt_1ms) UpdateMotor = 1;
38 
     CountMilliseconds++;
 38 
     CountMilliseconds++;
39 
    }  
 39 
    }
Line 40... Line 40...
40 
 
 40 
 
41 
     if(beeptime > 1)
 41 
     if(beeptime > 1)
42 
        {
 42 
        {
43 
        beeptime--;      
 43 
        beeptime--;
44 
        if(beeptime & BeepMuster)
 44 
        if(beeptime & BeepMuster)
45 
         {
 45 
         {
46 
          pieper_ein = 1;
 46 
          pieper_ein = 1;
47 
         }
 47 
         }
48 
         else pieper_ein = 0;
 48 
         else pieper_ein = 0;
49 
        }
 49 
        }
50 
     else
 50 
     else
51 
      {
 51 
      {
52 
       pieper_ein = 0;
 52 
       pieper_ein = 0;
53 
       BeepMuster = 0xffff;
 53 
       BeepMuster = 0xffff;
Line 54... Line 54...
54 
      }
 54 
      }
55 
 
 55 
 
56 
 
 56 
 
57 
     if(pieper_ein)
 57 
     if(pieper_ein)
58 
        {
 58 
        {
59 
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
 59 
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
60 
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
 60 
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
61 
        }
 61 
        }
62 
     else  
 62 
     else
63 
        {
 63 
        {
64 
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
 64 
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
65 
         else                      PORTC &= ~(1<<7);
 65 
         else                      PORTC &= ~(1<<7);
66 
        }
 66 
        }
67 
 
 67 
 
68 
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
 68 
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
69 
 {
 69 
 {
70 
  if(PINC & 0x10)
 70 
  if(PINC & 0x10)
71 
   {
 71 
   {
72 
    cntKompass++;
 72 
    cntKompass++;
73 
   }
 73 
   }
74 
  else
 74 
  else
75 
   {
 75 
   {
76 
    if((cntKompass) && (cntKompass < 362))
 76 
    if((cntKompass) && (cntKompass < 362))
77 
    {
 77 
    {
78 
     cntKompass += cntKompass / 41;
 78 
     cntKompass += cntKompass / 41;
79 
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
 79 
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
80 
    }
 80 
    }
81 
//     if(cntKompass < 10) cntKompass = 10;
 81 
//     if(cntKompass < 10) cntKompass = 10;
82 
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
 82 
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
83 
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
 83 
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
84 
    cntKompass = 0;
 84 
    cntKompass = 0;
Line 85... Line 85...
85 
   }
 85 
   }
86 
 }
 86 
 }
Line 98... Line 98...
98 
    //OCR1  = 0x00;
 98 
    //OCR1  = 0x00;
Line 99... Line 99...
99 
 
 99 
 
100 
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
 100 
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
101 
//    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);    // clk/256
 101 
//    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);    // clk/256
102 
    TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22);      // clk/64
 102 
    TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22);      // clk/64
103 
   
 103 
 
104 
 
 104 
 
Line 105... Line 105...
105 
  TIMSK2 |= _BV(OCIE2A);
 105 
  TIMSK2 |= _BV(OCIE2A);
106 
 
 106 
 
107 
    TIMSK0 |= _BV(TOIE0);
 107 
    TIMSK0 |= _BV(TOIE0);
108 
    OCR2A = 10;
 108 
    OCR2A = 10;
109 
    TCNT2 = 0;
 109 
    TCNT2 = 0;
Line 110... Line 110...
110 
   
 110 
 
Line 111... Line 111...
111 
}
 111 
}
112 
 
 112 
 
113 
// -----------------------------------------------------------------------
 113 
// -----------------------------------------------------------------------
114 
 
 114 
 
115 
unsigned int SetDelay (unsigned int t)
 115 
unsigned int SetDelay (unsigned int t)
116 
{
 116 
{
Line 117... Line 117...
117 
//  TIMSK0 &= ~_BV(TOIE0);
 117 
//  TIMSK0 &= ~_BV(TOIE0);
118 
  return(CountMilliseconds + t + 1);                                            
 118 
  return(CountMilliseconds + t + 1);
Line 140... Line 140...
140 
 unsigned int akt;
 140 
 unsigned int akt;
141 
 akt = SetDelay(w);
 141 
 akt = SetDelay(w);
142 
 while (!CheckDelay(akt)) ANALOG_ON;
 142 
 while (!CheckDelay(akt)) ANALOG_ON;
143 
}
 143 
}
Line 144... Line 144...
144 
 
 144 
 
145 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
145 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
146 
//  Servo ansteuern
 146 
//  Servo ansteuern
147 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++               
147 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
148 
SIGNAL(SIG_OVERFLOW2)
 148 
SIGNAL(SIG_OVERFLOW2)
149 
{
 149 
{
150 
  if (ServoState > 0) PORTD |= 0x80;
 150 
  if (ServoState > 0) PORTD |= 0x80;
151 
  else PORTD &= ~0x80;
 151 
  else PORTD &= ~0x80;
152 
  TCCR2A =3;
 152 
  TCCR2A =3;
153 
  TIMSK2 &= ~_BV(TOIE2);
 153 
  TIMSK2 &= ~_BV(TOIE2);
Line 154... Line 154...
154 
}
 154 
}
155 
 
 155 
 
156 
SIGNAL(SIG_OUTPUT_COMPARE2A)
 156 
SIGNAL(SIG_OUTPUT_COMPARE2A)
157 
{
 157 
{
158 
  static unsigned char postPulse = 0x80;
 158 
  static unsigned char postPulse = 0x80;
159 
  static int filterServo = 100;
 159 
  static int filterServo = 100;
160 
  #define MULTIPLIER 4
 160 
  #define MULTIPLIER 4
161 
  if(ServoState == 4)  
 161 
  if(ServoState == 4)
162 
    {
 162 
    {
163 
     ServoValue = 0x0030;  // Offset Part1
 163 
     ServoValue = 0x0030;  // Offset Part1
164 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
 164 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
165 
         ServoValue += filterServo;
 165 
         ServoValue += filterServo;
166 
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L )/ (512L/MULTIPLIER);
 166 
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
167 
     else ServoValue -= ((long) ((long)EE_Parameter.ServoNickComp * IntegralNick) / 128L) / (512L/MULTIPLIER);
 167 
     else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
168 
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
 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;
 169 
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
170 
   
 170 
 
171 
         DebugOut.Analog[20] = ServoValue;
 171 
         DebugOut.Analog[20] = ServoValue;
172 
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
 172 
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
173 
         OCR2A = 255-(ServoValue % 256);
 173 
         OCR2A = 255-(ServoValue % 256);
174 
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;  
 174 
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
175 
        }
 175 
        }
176 
  else if ((ServoState > 0) && (ServoState < 4))
 176 
  else if ((ServoState > 0) && (ServoState < 4))
177 
    {
 177 
    {
178 
          if(ServoValue > 255)
 178 
          if(ServoValue > 255)
179 
          { PORTD |= 0x80;
 179 
          { PORTD |= 0x80;
180 
                TCCR2A =3;
 180 
                TCCR2A =3;
181 
                ServoValue -= 255;
 181 
                ServoValue -= 255;
182 
          }
 182 
          }
183 
          else
 183 
          else
184 
      {  
 184 
      {
185 
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
 185 
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;
186 
                 OCR2A = postPulse; // Offset Part2
 186 
                 OCR2A = postPulse; // Offset Part2
187 
                 ServoState = 1;
 187 
                 ServoState = 1;
188 
      }
 188 
      }
189 
  }
 189 
  }
190 
  else if (ServoState == 0)
 190 
  else if (ServoState == 0)
191 
          {
 191 
          {
192 
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
 192 
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
193 
         PORTD&=~0x80;
 193 
         PORTD&=~0x80;
194 
                 TCCR2A = 3;
 194 
                 TCCR2A = 3;