Subversion Repositories FlightCtrl

Rev

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

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