Subversion Repositories FlightCtrl

Rev

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;
8
int ServoValue = 0;
136 salvo 9
//Salvo 8.9.2007
143 salvo 10
volatile uint8_t Kompass_Neuer_Wert= 0;
136 salvo 11
volatile unsigned int Kompass_Value_Old = 0;
12
// Salvo End
1 ingob 13
enum {
14
  STOP             = 0,
15
  CK               = 1,
16
  CK8              = 2,
17
  CK64             = 3,
18
  CK256            = 4,
19
  CK1024           = 5,
20
  T0_FALLING_EDGE  = 6,
21
  T0_RISING_EDGE   = 7
22
};
23
 
156 salvo 24
// Aenderungen von Peter Muehlenbrock ("Salvo") Stand 13.9.2007
25
/*
26
Driftkompensation fuer Gyros verbessert
27
Linearsensor mit fixem neutralwert
28
Ersatzkompass abgeleitet aus magnetkompass und Giergyro fuer nahezu neigungsubhaengige Kompassfunktion
29
*/
1 ingob 30
SIGNAL (SIG_OVERFLOW0)    // 8kHz
31
{
32
    static unsigned char cnt_1ms = 1,cnt = 0;
33
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
34
 
35
   if(!cnt--)
36
    {
37
     cnt = 9;
38
     cnt_1ms++;
39
     cnt_1ms %= 2;
40
     if(!cnt_1ms) UpdateMotor = 1;
41
     CountMilliseconds++;
42
     if(Timeout) Timeout--;
43
     }  
44
 
45
     if(beeptime > 1)
46
        {
47
        beeptime--;
48
        PORTD |= (1<<2);
49
        }
50
     else  
51
        PORTD &= ~(1<<2);
52
 
136 salvo 53
// if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
1 ingob 54
 {
55
  if(PINC & 0x10)
56
   {
57
    cntKompass++;
58
   }
59
  else
60
   {
61
    if((cntKompass) && (cntKompass < 4000))
62
    {
104 salvo 63
// Salvo Kompassoffset 30.8.2007 ***********
157 salvo 64
         Kompass_Value_Old      =       KompassValue;
104 salvo 65
     KompassValue = cntKompass -KOMPASS_OFFSET;
66
 
67
         if (KompassValue < 0)
68
         {
69
                KompassValue += 360;
70
         }
71
         if (KompassValue >= 360)
72
         {
73
                KompassValue -= 360;
74
         }     
75
 // Salvo End  
1 ingob 76
    }
77
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
156 salvo 78
//Salvo 13.9.2007 Ok Erkennung des Magnetkompasses 
136 salvo 79
         Kompass_Neuer_Wert     = 1;
156 salvo 80
 // Salvo End
136 salvo 81
     cntKompass = 0;
1 ingob 82
   }
83
 }
84
}
85
 
86
 
87
void Timer_Init(void)
88
{
89
    tim_main = SetDelay(10);
90
    TCCR0B = CK8;
91
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
92
    OCR0A =  0;
93
    OCR0B = 120;
94
    TCNT0 = -TIMER_RELOAD_VALUE;  // reload
95
    //OCR1  = 0x00;
96
 
97
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
98
    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
99
 
100
//    TIMSK2 |= _BV(TOIE2);
101
TIMSK2 |= _BV(OCIE2A);
102
 
103
    TIMSK0 |= _BV(TOIE0);
104
    OCR2A = 10;
105
    TCNT2 = 0;
106
 
107
}
108
 
109
// -----------------------------------------------------------------------
110
 
111
unsigned int SetDelay (unsigned int t)
112
{
113
//  TIMSK0 &= ~_BV(TOIE0);
114
  return(CountMilliseconds + t + 1);                                            
115
//  TIMSK0 |= _BV(TOIE0);
116
}
117
 
118
// -----------------------------------------------------------------------
119
char CheckDelay(unsigned int t)
120
{
121
//  TIMSK0 &= ~_BV(TOIE0);
122
  return(((t - CountMilliseconds) & 0x8000) >> 9);
123
//  TIMSK0 |= _BV(TOIE0);
124
}
125
 
126
// -----------------------------------------------------------------------
127
void Delay_ms(unsigned int w)
128
{
129
 unsigned int akt;
130
 akt = SetDelay(w);
131
 while (!CheckDelay(akt));
132
}
133
 
134
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
135
//  Servo ansteuern
136
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
137
SIGNAL(SIG_OUTPUT_COMPARE2A)
138
{
139
  static unsigned char timer = 10;
140
 
141
  if(!timer--)  
142
    {
143
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
144
     ServoValue =  Parameter_ServoNickControl;
145
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
146
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
147
 
148
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
149
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
150
 
143 salvo 151
//     DebugOut.Analog[10] = ServoValue;     
1 ingob 152
     OCR2A = ServoValue;// + 75;
153
     timer = EE_Parameter.ServoNickRefresh;
154
    }
155
    else
156
    {
157
     TCCR2A =3;
158
     PORTD&=~0x80;
159
    }
160
}