Subversion Repositories FlightCtrl

Rev

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

Rev Author Line No. Line
1 ingob 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
3
// + only for non-profit use
4
// + www.MikroKopter.com
5
// + see the File "License.txt" for further Informations
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
 
8
#include "main.h"
9
volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
1166 hbuss 10
volatile int  AdWertNickFilter = 0, AdWertRollFilter = 0, AdWertGierFilter = 0;
11
volatile int  HiResNick = 2500, HiResRoll = 2500;
380 hbuss 12
volatile int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
13
volatile int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
1166 hbuss 14
volatile char messanzahl_AccHoch = 0;
1 ingob 15
volatile long Luftdruck = 32000;
16
volatile int  StartLuftdruck;
17
volatile unsigned int  MessLuftdruck = 1023;
18
unsigned char DruckOffsetSetting;
1036 hbuss 19
signed char ExpandBaro = 0;
1 ingob 20
volatile int HoeheD = 0;
21
volatile char messanzahl_Druck;
22
volatile int  tmpLuftdruck;
23
volatile unsigned int ZaehlMessungen = 0;
918 hbuss 24
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
25
unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
1168 hbuss 26
volatile unsigned char AdReady = 1;
1 ingob 27
//#######################################################################################
28
//
29
void ADC_Init(void)
30
//#######################################################################################
31
{
32
    ADMUX = 0;//Referenz ist extern
1155 hbuss 33
    ANALOG_ON;
1 ingob 34
}
35
 
36
void SucheLuftruckOffset(void)
37
{
38
 unsigned int off;
173 holgerb 39
 off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
40
 if(off > 20) off -= 10;
41
 OCR0A = off;
1036 hbuss 42
 ExpandBaro = 0;
380 hbuss 43
 Delay_ms_Mess(100);
173 holgerb 44
 if(MessLuftdruck < 850) off = 0;
45
 for(; off < 250;off++)
1 ingob 46
  {
47
  OCR0A = off;
380 hbuss 48
  Delay_ms_Mess(50);
1 ingob 49
  printf(".");  
1036 hbuss 50
  if(MessLuftdruck < 850) break;
1 ingob 51
  }
173 holgerb 52
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
53
 DruckOffsetSetting = off;
380 hbuss 54
 Delay_ms_Mess(300);
1 ingob 55
}
56
 
918 hbuss 57
void SucheGyroOffset(void)
58
{
59
 unsigned char i, ready = 0;
60
 GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
61
 for(i=140; i != 0; i--)
62
  {
921 hbuss 63
   if(ready == 3 && i > 10) i = 9;
918 hbuss 64
   ready = 0;
65
   if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
66
   if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
67
   if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
68
   twi_state = 8;  
69
   i2c_start();
70
   if(AnalogOffsetNick < 10)  { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};
71
   if(AnalogOffsetRoll < 10)  { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};
72
   if(AnalogOffsetGier < 10)  { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
73
   while(twi_state);
74
   messanzahl_Druck = 0;
75
   ANALOG_ON;
76
   while(messanzahl_Druck == 0);
77
   if(i<10) Delay_ms_Mess(10);  
78
  }
921 hbuss 79
   Delay_ms_Mess(70);  
918 hbuss 80
}
1 ingob 81
 
1171 hbuss 82
/*
83
 
84
1  r
85
2     g
86
3     y
87
4     x
88
5  n
89
6  r
90
7     u
91
8     z
92
9     L
93
10 n  
94
11 r
95
12    g
96
13    y
97
14    x
98
15 n
99
16 r
100
17    L
101
*/
102
 
1 ingob 103
//#######################################################################################
104
//
105
SIGNAL(SIG_ADC)
106
//#######################################################################################
107
{
108
    static unsigned char kanal=0,state = 0;
1171 hbuss 109
    static signed int gier1, roll1, nick1, nick_filter, roll_filter;   
110
        static signed int accy, accx;
111
    switch(state++)
112
        {
113
        case 0:
114
            nick1 = ADC;
115
            kanal = AD_ROLL;
116
            break;
117
        case 1:
118
            roll1 = ADC;
119
                    kanal = AD_GIER;
120
            break;
121
        case 2:
122
            gier1 = ADC;
123
            kanal = AD_ACC_Y;
124
            break;
125
        case 3:
126
            Aktuell_ay = NeutralAccY - ADC;
127
            accy = Aktuell_ay;
128
                    kanal = AD_ACC_X;
129
            break;
130
        case 4:
131
            Aktuell_ax = ADC - NeutralAccX;
132
            accx =  Aktuell_ax;
133
            kanal = AD_NICK;
134
            break;
135
        case 5:
136
            nick1 += ADC;
137
            kanal = AD_ROLL;
138
            break;
139
        case 6:
140
            roll1 += ADC;
141
            kanal = AD_UBAT;
142
            break;
143
        case 7:
144
            UBat = (3 * UBat + ADC / 3) / 4;
145
                    kanal = AD_ACC_Z;
146
            break;
147
       case 8:
148
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
149
            if(AdWertAccHoch > 1)
150
             {
151
              if(NeutralAccZ < 750)
152
               {
153
                NeutralAccZ += 0.02;
154
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
155
               }
156
             }  
157
             else if(AdWertAccHoch < -1)
158
             {
159
              if(NeutralAccZ > 550)
160
                {
161
                 NeutralAccZ-= 0.02;
162
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
163
                }
164
             }
165
            messanzahl_AccHoch = 1;
166
            Aktuell_az = ADC;
167
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
168
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
169
                kanal = AD_DRUCK;
170
            break;
171
   // "case 8:" fehlt hier absichtlich
172
        case 10:
173
            nick1 += ADC;
174
            kanal = AD_ROLL;
175
            break;
176
        case 11:
177
            roll1 += ADC;
178
                    kanal = AD_GIER;
179
            break;
180
        case 12:
181
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 1) / 2;
182
            else
183
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1);
184
                        else                                       AdWertGier = (ADC + gier1);
185
            kanal = AD_ACC_Y;
186
            break;
187
        case 13:
188
            Aktuell_ay = NeutralAccY - ADC;
189
            AdWertAccRoll = (Aktuell_ay + accy);
190
            kanal = AD_ACC_X;
191
            break;
192
        case 14:
193
            Aktuell_ax = ADC - NeutralAccX;
194
            AdWertAccNick =  (Aktuell_ax + accx);
195
            kanal = AD_NICK;
196
            break;
197
        case 15:
198
            nick1 += ADC;
199
            AdWertNick = nick1 / 2;
200
            nick_filter = (nick_filter + 4 * nick1) / 2;
201
                        if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
202
            HiResNick = nick_filter - 8 * AdNeutralNick;
203
            AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
204
//            DebugOut.Analog[21] = AdWertNickFilter / 4;  
205
            kanal = AD_ROLL;
206
            break;
207
        case 16:
208
            roll1 += ADC;
209
            AdWertRoll = roll1 / 2;
210
            roll_filter = (roll_filter + 4 * roll1) / 2;
211
            if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
212
            HiResRoll = roll_filter - 8 * AdNeutralRoll;
213
            AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
214
//            DebugOut.Analog[22] = AdWertRollFilter / 4;
215
                kanal = AD_DRUCK;
216
            break;
217
        case 17:
218
            state = 0;
219
                        AdReady = 1;
220
            ZaehlMessungen++;
221
            // "break" fehlt hier absichtlich
222
        case 9:
223
            tmpLuftdruck += ADC;
224
            if(++messanzahl_Druck >= 5)
225
                {
226
                                tmpLuftdruck /= 2;
227
                MessLuftdruck = ADC;
228
                messanzahl_Druck = 0;
229
                                HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/32;  // D-Anteil = neuerWert - AlterWert
230
DebugOut.Analog[24] = 255 * ExpandBaro + StartLuftdruck - tmpLuftdruck;
231
DebugOut.Analog[23] = HoeheD;
232
                Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
233
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
234
                tmpLuftdruck /= 2;
235
                }
236
            kanal = AD_NICK;
237
            break;
238
        default:  
239
            kanal = 0; state = 0; kanal = AD_NICK;
240
            break;
241
        }
242
    ADMUX = kanal;
243
    if(state != 0) ANALOG_ON;
244
}
245
 
246
 
247
 
248
/*
249
//#######################################################################################
250
//
251
SIGNAL(SIG_ADC)
252
//#######################################################################################
253
{
254
    static unsigned char kanal=0,state = 0;
1166 hbuss 255
    static signed int gier1, roll1, nick1;
256
        static signed long nick_filter, roll_filter;   
257
        static signed int accy, accx;
258
    switch(state++)
259
        {
260
        case 0:
261
            nick1 = ADC;
262
            kanal = AD_ROLL;
263
            break;
264
        case 1:
265
            roll1 = ADC;
266
                    kanal = AD_GIER;
267
            break;
268
        case 2:
269
            gier1 = ADC;
270
            kanal = AD_ACC_Y;
271
            break;
272
        case 3:
273
            Aktuell_ay = NeutralAccY - ADC;
274
            accy = Aktuell_ay;
275
            kanal = AD_NICK;
276
            break;
277
        case 4:
278
            nick1 += ADC;
279
            kanal = AD_ROLL;
280
            break;
281
        case 5:
282
            roll1 += ADC;
283
                    kanal = AD_ACC_Z;
284
            break;
285
       case 6:
286
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
287
            if(AdWertAccHoch > 1)
288
             {
289
              if(NeutralAccZ < 750)
290
               {
291
                NeutralAccZ += 0.02;
292
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
293
               }
294
             }  
295
             else if(AdWertAccHoch < -1)
296
             {
297
              if(NeutralAccZ > 550)
298
                {
299
                 NeutralAccZ-= 0.02;
300
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
301
                }
302
             }
303
            messanzahl_AccHoch = 1;
304
            Aktuell_az = ADC;
305
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
306
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
307
            kanal = AD_NICK;
308
            break;
309
        case 7:
310
            nick1 += ADC;
311
            kanal = AD_ROLL;
312
            break;
313
        case 8:
314
            roll1 += ADC;
315
                    kanal = AD_ACC_X;
316
            break;
317
        case 9:
318
            Aktuell_ax = ADC - NeutralAccX;
319
            accx =  Aktuell_ax;
320
                    kanal = AD_GIER;
321
            break;
322
        case 10:
323
            gier1 += ADC;
324
            kanal = AD_NICK;
325
            break;
326
        case 11:
327
            nick1 += ADC;
328
            kanal = AD_ROLL;
329
            break;
330
        case 12:
331
            roll1 += ADC;
332
            kanal = AD_UBAT;
333
            break;
334
        case 13:
335
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
336
            kanal = AD_ACC_Y;
337
            break;
338
        case 14:
339
            Aktuell_ay = NeutralAccY - ADC;
340
            accy += Aktuell_ay;
341
            kanal = AD_NICK;
342
            break;
343
        case 15:
344
            nick1 += ADC;
345
            kanal = AD_ROLL;
346
            break;
347
        case 16:
348
            roll1 += ADC;
349
            kanal = AD_ACC_X;
350
            break;
351
        case 17:
352
            Aktuell_ax = ADC - NeutralAccX;
353
            accx += Aktuell_ax;
354
                    kanal = AD_NICK;
355
            break;
356
        case 18:
357
            nick1 += ADC;
358
            kanal = AD_ROLL;
359
            break;
360
        case 19:
361
            roll1 += ADC;
362
                    kanal = AD_GIER;
363
            break;
364
        case 20:
365
            gier1 += ADC;
366
            kanal = AD_ACC_Y;
367
            break;
368
        case 21:
369
            Aktuell_ay = NeutralAccY - ADC;
370
            accy += Aktuell_ay;
371
            kanal = AD_NICK;
372
            break;
373
        case 22:
374
            nick1 += ADC;
375
            kanal = AD_ROLL;
376
            break;
377
        case 23:
378
            roll1 += ADC;
379
                kanal = AD_DRUCK;
380
            break;
381
        case 24:
382
            tmpLuftdruck += ADC;
383
            if(++messanzahl_Druck >= 5)
384
                {
385
                MessLuftdruck = ADC;
386
                messanzahl_Druck = 0;
1167 hbuss 387
                                HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8;  // D-Anteil = neuerWert - AlterWert
1166 hbuss 388
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
389
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
390
                tmpLuftdruck = 0;
391
                }
392
            kanal = AD_NICK;
393
            break;
394
        case 25:
395
            nick1 += ADC;
396
            kanal = AD_ROLL;
397
            break;
398
        case 26:
399
            roll1 += ADC;
400
                    kanal = AD_ACC_X;
401
            break;
402
        case 27:
403
            Aktuell_ax = ADC - NeutralAccX;
404
            accx +=  Aktuell_ax;
405
                    kanal = AD_GIER;
406
            break;
407
        case 28:
408
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 2) / 4;
409
            else
410
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
411
                        else                                       AdWertGier = (ADC + gier1 + 1) / 2;
412
            kanal = AD_NICK;
413
            break;
414
        case 29:
415
            nick1 += ADC;
416
            kanal = AD_ROLL;
417
            break;
418
        case 30:
419
            roll1 += ADC;
420
            kanal = AD_ACC_Y;
421
            break;
422
        case 31:
423
            Aktuell_ay = NeutralAccY - ADC;
424
            AdWertAccRoll = (Aktuell_ay + accy);
425
            kanal = AD_NICK;
426
            break;
427
        case 32:
428
            AdWertNick = (ADC + nick1 + 3) / 5;
429
            nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
430
            if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
431
            HiResNick = nick_filter - 20 * AdNeutralNick;
432
            AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
433
            DebugOut.Analog[21] = AdWertNickFilter / 4;  
434
            kanal = AD_ROLL;
435
            break;
436
        case 33:
437
            AdWertRoll = (ADC + roll1 + 3) / 5;
438
            roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
439
            if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
440
            HiResRoll = roll_filter - 20 * AdNeutralRoll;
441
            AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
442
            DebugOut.Analog[22] = AdWertRollFilter / 4;
443
            kanal = AD_ACC_X;
444
            break;
445
        case 34:
446
            Aktuell_ax = ADC - NeutralAccX;
447
            AdWertAccNick =  (Aktuell_ax + accx);
448
                    kanal = AD_NICK;
449
            state = 0;
450
                        AdReady = 1;
451
            ZaehlMessungen++;
452
            break;
453
        default:
454
            kanal = 0;
455
            state = 0;
456
            break;
457
        }
458
    ADMUX = kanal;
459
    if(state != 0) ANALOG_ON;
460
}
1171 hbuss 461
*/