Subversion Repositories FlightCtrl

Rev

Rev 1127 | Go to most recent revision | Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1112 thjac 1
 
2
#include "main.h"
3
#include "parameter.h"
4
 
5
volatile unsigned int CountMilliseconds = 0;
6
volatile static unsigned int tim_main;
7
volatile unsigned char UpdateMotor = 0;
8
volatile unsigned int cntKompass = 0;
9
volatile unsigned int beeptime = 0;
10
volatile unsigned char SendSPI = 0;
11
volatile unsigned int ServoState = 40;
12
 
13
unsigned int BeepMuster = 0xffff;
14
unsigned int ServoValue = 0;
15
 
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
 
28
SIGNAL (SIG_OVERFLOW0)    // 8kHz
29
{
30
    static unsigned char cnt_1ms = 1,cnt = 0;
31
    unsigned char pieper_ein = 0;
32
//    TCNT0 -= 250;//TIMER_RELOAD_VALUE;
33
   if(SendSPI) SendSPI--;
34
   if(!cnt--)
35
    {
36
     cnt = 9;
37
     cnt_1ms++;
38
     cnt_1ms %= 2;
39
     if(!cnt_1ms) UpdateMotor = 1;
40
     CountMilliseconds++;
41
    }  
42
 
43
     if(beeptime > 1)
44
        {
45
        beeptime--;      
46
        if(beeptime & BeepMuster)
47
         {
48
          pieper_ein = 1;
49
         }
50
         else pieper_ein = 0;
51
        }
52
     else
53
      {
54
       pieper_ein = 0;
55
       BeepMuster = 0xffff;
56
      }
57
 
58
 
59
     if(pieper_ein)
60
        {
61
          if(PlatinenVersion == 10) PORTD |= (1<<2); // Speaker an PORTD.2
62
          else                      PORTC |= (1<<7); // Speaker an PORTC.7
63
        }
64
     else  
65
        {
66
         if(PlatinenVersion == 10) PORTD &= ~(1<<2);
67
         else                      PORTC &= ~(1<<7);
68
        }
69
 
70
 if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)
71
 {
72
  if(PINC & 0x10)
73
   {
74
    cntKompass++;
75
   }
76
  else
77
   {
78
    if((cntKompass) && (cntKompass < 362))
79
    {
80
     cntKompass += cntKompass / 41;
81
     if(cntKompass > 10) KompassValue = cntKompass - 10; else KompassValue = 0;
82
    }
83
//     if(cntKompass < 10) cntKompass = 10;
84
//     KompassValue = (unsigned long)((unsigned long)(cntKompass-10)*720L + 1L) / 703L;
85
     KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180;
86
    cntKompass = 0;
87
   }
88
 }
89
}
90
 
91
//----------------------------
92
void Timer_Init(void)
93
{
94
    tim_main = SetDelay(10);
95
    TCCR0B = CK8;
96
    TCCR0A = (1<<COM0A1)|(1<<COM0B1)|3;//fast PWM
97
    OCR0A =  0;
98
    OCR0B = 120;
99
    TCNT0 = (unsigned char)-TIMER_RELOAD_VALUE;  // reload
100
    //OCR1  = 0x00;
101
 
102
    TCCR2A=(1<<COM2A1)|(1<<COM2A0)|3;
103
//    TCCR2B=(0<<CS20)|(1<<CS21)|(1<<CS22);    // clk/256
104
    TCCR2B=(0<<CS20)|(0<<CS21)|(1<<CS22);      // clk/64
105
 
106
 
107
  TIMSK2 |= _BV(OCIE2A);
108
 
109
    TIMSK0 |= _BV(TOIE0);
110
    OCR2A = 10;
111
    TCNT2 = 0;
112
 
113
}
114
 
115
// -----------------------------------------------------------------------
116
 
117
unsigned int SetDelay (unsigned int t)
118
{
119
//  TIMSK0 &= ~_BV(TOIE0);
120
  return(CountMilliseconds + t + 1);                                            
121
//  TIMSK0 |= _BV(TOIE0);
122
}
123
 
124
// -----------------------------------------------------------------------
125
char CheckDelay(unsigned int t)
126
{
127
//  TIMSK0 &= ~_BV(TOIE0);
128
  return(((t - CountMilliseconds) & 0x8000) >> 9);
129
//  TIMSK0 |= _BV(TOIE0);
130
}
131
 
132
// -----------------------------------------------------------------------
133
void Delay_ms(unsigned int w)
134
{
135
 unsigned int akt;
136
 akt = SetDelay(w);
137
 while (!CheckDelay(akt));
138
}
139
 
140
void Delay_ms_Mess(unsigned int w)
141
{
142
 unsigned int akt;
143
 akt = SetDelay(w);
144
 while (!CheckDelay(akt)) ANALOG_ON;
145
}
146
 
147
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
148
//  Servo ansteuern
149
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++                
150
SIGNAL(SIG_OVERFLOW2) {
151
 
152
        if (ServoState > 0)
153
                PORTD |= 0x80;
154
        else
155
                PORTD &= ~0x80;
156
 
157
        TCCR2A  = 3;
158
        TIMSK2 &= ~_BV( TOIE2 );
159
}
160
 
161
SIGNAL(SIG_OUTPUT_COMPARE2A) {
162
 
163
        static unsigned char postPulse   = 0x80;
164
        static int           filterServo = 100;
165
 
166
        if( ServoState == 4 ) {
167
 
168
                ServoValue  = 0x0030;  // Offset Part1
169
                filterServo = ( filterServo * 3 + (int) Parameter_ServoNickControl * 2 ) >> DIV_4;
170
                ServoValue += filterServo;
171
 
172
                // Min und Max vorverlegt, damit sich diese auf ServoNickControl beziehen und ggf. noch Nick-kompensiert werden
173
                if( ServoValue < ( (int) EE_Parameter.ServoNickMin * 3 ) )
174
                        ServoValue = (int) EE_Parameter.ServoNickMin * 3;
175
                else if( ServoValue > ( (int) EE_Parameter.ServoNickMax * 3 ) )
176
                        ServoValue = (int) EE_Parameter.ServoNickMax * 3;
177
 
178
                long integral;
179
 
180
                /* Über Parameter läßt sich zwischen "+" und "X" - Formations
181
                 * umschalten (sh. parameter.h)
182
                 */
183
                if( PARAM_X_FORMATION ) {
184
                        integral = IntegralNick - IntegralRoll;
185
                } else {
186
                        integral = IntegralNick;
187
                }
188
 
189
                if( EE_Parameter.ServoNickCompInvert & 0x01 )
190
                        ServoValue += ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 );
191
                else
192
                        ServoValue -= ( (long) ( (long) EE_Parameter.ServoNickComp * integral ) >> DIV_128 ) / ( 512L >> DIV_4 );
193
 
194
                DebugOut.Analog[20] = ServoValue;
195
 
196
                if ( ( ServoValue % 255 ) < 45 ) {
197
                        ServoValue += 77;
198
                        postPulse   = 0x60 - 77;
199
                } else {
200
                        postPulse   = 0x60;
201
                }
202
 
203
                OCR2A  = 255 - ( ServoValue % 256 );
204
                TCCR2A = (1 << COM2A1 ) | ( 1 << COM2A0 ) | 3;  
205
 
206
        } else if( ( ServoState > 0 ) && ( ServoState < 4 ) ) {
207
 
208
                if( ServoValue > 255 ) {
209
                        PORTD      |= 0x80;
210
                        TCCR2A      = 3;
211
                        ServoValue -= 255;
212
                } else {         
213
                        TCCR2A     = ( 1 << COM2A1 ) | ( 0 << COM2A0 ) | 3;  
214
                        OCR2A      = postPulse; // Offset Part2
215
                        ServoState = 1;
216
                }
217
 
218
        } else if( ServoState == 0 ) {
219
                ServoState  = (int) EE_Parameter.ServoNickRefresh << MUL_4;
220
                PORTD      &= ~0x80;
221
                TCCR2A      = 3;
222
        }
223
 
224
        ServoState--;
225
}