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