Subversion Repositories FlightCtrl

Rev

Rev 308 | Rev 312 | 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
                {
309 osiair 63
                        KompassValue = (MM3_heading());
64
 
65
                if (KompassValue < 0 ) { KompassValue = 380+MM3_heading();}
66
                if (KompassValue > 359) { KompassValue = 359;}
67
 
68
 
69
                KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360);
70
 
71
 
308 osiair 72
                        cntKompass = 320;
73
                }
74
        }
75
 
76
 
77
}
78
 
79
 
80
void Timer_Init(void)
81
{
82
    tim_main = SetDelay(10);
83
    TCCR0B = CK8;
84
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
85
    OCR0A =  0;
86
    OCR0B = 120;
87
    TCNT0 = -TIMER_RELOAD_VALUE;  // reload
88
    //OCR1  = 0x00;
89
 
90
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
91
    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);
92
 
93
//    TIMSK2 |= _BV(TOIE2);
94
TIMSK2 |= _BV(OCIE2A);
95
 
96
    TIMSK0 |= _BV(TOIE0);
97
    OCR2A = 10;
98
    TCNT2 = 0;
99
 
100
}
101
 
102
// -----------------------------------------------------------------------
103
 
104
unsigned int SetDelay (unsigned int t)
105
{
106
//  TIMSK0 &= ~_BV(TOIE0);
107
  return(CountMilliseconds + t + 1);                                            
108
//  TIMSK0 |= _BV(TOIE0);
109
}
110
 
111
// -----------------------------------------------------------------------
112
char CheckDelay(unsigned int t)
113
{
114
//  TIMSK0 &= ~_BV(TOIE0);
115
  return(((t - CountMilliseconds) & 0x8000) >> 9);
116
//  TIMSK0 |= _BV(TOIE0);
117
}
118
 
119
// -----------------------------------------------------------------------
120
void Delay_ms(unsigned int w)
121
{
122
 unsigned int akt;
123
 akt = SetDelay(w);
124
 while (!CheckDelay(akt));
125
}
126
 
127
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
128
//  Servo ansteuern
129
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
130
SIGNAL(SIG_OUTPUT_COMPARE2A)
131
{
132
  static unsigned char timer = 10;
133
 
134
  if(!timer--)  
135
    {
136
     TCCR2A=(1<<COM2A1)|(0<<COM2A0)|3;  
137
     ServoValue =  Parameter_ServoNickControl;
138
     if(EE_Parameter.ServoNickCompInvert & 0x01) ServoValue += ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
139
     else ServoValue -= ((long) EE_Parameter.ServoNickComp * (IntegralNick / 128)) / 512;
140
 
141
     if(ServoValue < EE_Parameter.ServoNickMin) ServoValue = EE_Parameter.ServoNickMin;
142
     else if(ServoValue > EE_Parameter.ServoNickMax) ServoValue = EE_Parameter.ServoNickMax;
143
 
144
     //DebugOut.Analog[10] = ServoValue;     
145
     OCR2A = ServoValue;// + 75;
146
     timer = EE_Parameter.ServoNickRefresh;
147
    }
148
    else
149
    {
150
     TCCR2A =3;
151
     PORTD&=~0x80;
152
    }
153
}