Subversion Repositories FlightCtrl

Rev

Rev 1378 | Details | Compare with Previous | Last modification | View Log | RSS feed

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