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