Subversion Repositories FlightCtrl

Rev

Go to most recent revision | Details | 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;
1105 killagreg 12 
int ServoValue = 0;
1 ingob 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;
173 holgerb 29 
    unsigned char pieper_ein = 0;
1 ingob 30 
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
723 hbuss 31 
   if(SendSPI) SendSPI--;
1 ingob 32 
   if(!cnt--)
33 
    {
1105 killagreg 34 
     cnt = 9;
1 ingob 35 
     cnt_1ms++;
36 
     cnt_1ms %= 2;
37 
     if(!cnt_1ms) UpdateMotor = 1;
38 
     CountMilliseconds++;
1105 killagreg 39 
    }
1 ingob 40 
 
41 
     if(beeptime > 1)
42 
        {
1105 killagreg 43 
        beeptime--;
44 
        if(beeptime & BeepMuster)
173 holgerb 45 
         {
46 
          pieper_ein = 1;
47 
         }
48 
         else pieper_ein = 0;
1 ingob 49 
        }
1105 killagreg 50 
     else
173 holgerb 51 
      {
52 
       pieper_ein = 0;
53 
       BeepMuster = 0xffff;
1105 killagreg 54 
      }
173 holgerb 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 
        }
1105 killagreg 62 
     else
173 holgerb 63 
        {
64 
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
65 
         else                      PORTC &= ~(1<<7);
1105 killagreg 66 
        }
67 
 
1 ingob 68 
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
69 
 {
70 
  if(PINC & 0x10)
71 
   {
1105 killagreg 72 
    cntKompass++;
1 ingob 73 
   }
74 
  else
75 
   {
1105 killagreg 76 
    if((cntKompass) && (cntKompass < 362))
77 
    {
693 hbuss 78 
     cntKompass += cntKompass / 41;
79 
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
1105 killagreg 80 
    }
1 ingob 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;
1105 killagreg 85 
   }
1 ingob 86 
 }
87 
}
88 
 
910 hbuss 89 
//----------------------------
1 ingob 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;
304 ingob 97 
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
1 ingob 98 
    //OCR1  = 0x00;
99 
 
100 
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
910 hbuss 101 
//    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);    // clk/256
102 
    TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22);      // clk/64
1105 killagreg 103 
 
104 
 
910 hbuss 105 
  TIMSK2 |= _BV(OCIE2A);
1 ingob 106 
 
107 
    TIMSK0 |= _BV(TOIE0);
108 
    OCR2A = 10;
109 
    TCNT2 = 0;
1105 killagreg 110 
 
1 ingob 111 
}
112 
 
113 
// -----------------------------------------------------------------------
114 
 
115 
unsigned int SetDelay (unsigned int t)
116 
{
117 
//  TIMSK0 &= ~_BV(TOIE0);
1105 killagreg 118 
  return(CountMilliseconds + t + 1);
1 ingob 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 
 
395 hbuss 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 
 
1105 killagreg 145 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1 ingob 146 
//  Servo ansteuern
1105 killagreg 147 
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
910 hbuss 148 
SIGNAL(SIG_OVERFLOW2)
149 
{
1105 killagreg 150 
  if (ServoState > 0) PORTD |= 0x80;
151 
  else PORTD &= ~0x80;
910 hbuss 152 
  TCCR2A =3;
153 
  TIMSK2 &= ~_BV(TOIE2);
154 
}
155 
 
1 ingob 156 
SIGNAL(SIG_OUTPUT_COMPARE2A)
157 
{
910 hbuss 158 
  static unsigned char postPulse = 0x80;
159 
  static int filterServo = 100;
1155 hbuss 160 
//  static unsigned char restPulse = 50;
910 hbuss 161 
  #define MULTIPLIER 4
1111 hbuss 162 
  if(PlatinenVersion < 20)
163 
  {
164 
   if(ServoState == 4)
1 ingob 165 
    {
910 hbuss 166 
     ServoValue = 0x0030;  // Offset Part1
167 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
168 
         ServoValue += filterServo;
1105 killagreg 169 
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
170 
     else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
910 hbuss 171 
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
172 
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
1105 killagreg 173 
 
910 hbuss 174 
         DebugOut.Analog[20] = ServoValue;
175 
         if ((ServoValue % 255) < 45) { ServoValue+= 77; postPulse = 0x60 - 77; } else postPulse = 0x60;
1105 killagreg 176 
         OCR2A = 255-(ServoValue % 256);
177 
         TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
910 hbuss 178 
        }
1111 hbuss 179 
    else if ((ServoState > 0) && (ServoState < 4))
910 hbuss 180 
    {
1105 killagreg 181 
          if(ServoValue > 255)
182 
          { PORTD |= 0x80;
910 hbuss 183 
                TCCR2A =3;
1105 killagreg 184 
                ServoValue -= 255;
910 hbuss 185 
          }
186 
          else
1105 killagreg 187 
      {
188 
             TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;
910 hbuss 189 
                 OCR2A = postPulse; // Offset Part2
190 
                 ServoState = 1;
1105 killagreg 191 
      }
1111 hbuss 192 
    }
193 
    else if (ServoState == 0)
1105 killagreg 194 
          {
910 hbuss 195 
             ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
196 
         PORTD&=~0x80;
197 
                 TCCR2A = 3;
198 
          }
1111 hbuss 199 
  }
200 
 else
201 
  {
202 
  if(ServoState == 4)
203 
    {
204 
     PORTD &= ~0x80;
205 
     PORTC |= _BV(PC6);
206 
     ServoValue = 0x00030;  // Offset Part1
207 
     filterServo = (filterServo * 3 + (int) Parameter_ServoNickControl * 2)/4;
208 
     ServoValue += filterServo;
209 
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
210 
     else ServoValue -= (int)( ( (long)EE_Parameter.ServoNickComp * (IntegralNick / 128L) ) / (512L/MULTIPLIER) );
211 
     if((ServoValue) < ((int)EE_Parameter.ServoNickMin*3)) ServoValue = (int)EE_Parameter.ServoNickMin*3;
212 
     else if((ServoValue) > ((int)EE_Parameter.ServoNickMax*3)) ServoValue = (int)EE_Parameter.ServoNickMax*3;
213 
    PORTD |= 0x80;     // dummy clock to skip output Q0
214 
  if ((ServoValue % 256) < 1)  ServoValue -=2;
215 
  if ((ServoValue % 256) > 253)  ServoValue +=2;
216 
  DebugOut.Analog[20] = ServoValue;
217 
 
218 
  OCR2A = 254-(ServoValue % 255);
219 
  PORTD &= ~0x80;
220 
 
221 
  TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;    // set on match
222 
  //ServoValue -= OCR2A;
223 
  PORTC &= ~_BV(PC6);
224 
    }
225 
 else if ((ServoState > 0) && (ServoState < 4))
226 
    {
227 
   if(ServoValue > 255)
228 
   {
229 
    PORTD &= ~0x80;
230 
    TCCR2A =3;
231 
    ServoValue -= 255;
232 
    OCR2A = postPulse; // Offset Part2
233 
    //OCR2A = (ServoValue % 256);
234 
     }
235 
   else
236 
      {
237 
      OCR2A = postPulse; // Offset Part2
238 
      //OCR2A = (ServoValue % 256);
239 
      //TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
240 
      ServoState = 1;
241 
      }
242 
    }
243 
  else if (ServoState == 0)
244 
    {
245 
     PORTD &= ~0x80;
246 
     TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
247 
     ServoState  = (int) EE_Parameter.ServoNickRefresh * MULTIPLIER;
248 
     TCCR2A =3;
249 
    }
250 
 }
1153 hbuss 251 
 ServoState--;
1111 hbuss 252 
}
1 ingob 253