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