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