Subversion Repositories FlightCtrl

Rev

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