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