Subversion Repositories FlightCtrl

Rev

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