Subversion Repositories FlightCtrl

Rev

Rev 1622 | Rev 1639 | Go to most recent revision | 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"
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;
1638 holgerb 57
 OCR0A = off;
380 hbuss 58
 Delay_ms_Mess(300);
1638 holgerb 59
 
1 ingob 60
}
61
 
918 hbuss 62
void SucheGyroOffset(void)
63
{
64
 unsigned char i, ready = 0;
1219 hbuss 65
 int timeout;
918 hbuss 66
 GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
1219 hbuss 67
 timeout = SetDelay(2000);
918 hbuss 68
 for(i=140; i != 0; i--)
69
  {
921 hbuss 70
   if(ready == 3 && i > 10) i = 9;
918 hbuss 71
   ready = 0;
72
   if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
73
   if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
74
   if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
1638 holgerb 75
   twi_state = 18;
1246 killagreg 76
   i2c_start();
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;
1171 hbuss 117
        static signed int accy, accx;
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:
158
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
1246 killagreg 159
            if(AdWertAccHoch > 1)
1171 hbuss 160
             {
1246 killagreg 161
              if(NeutralAccZ < 750)
1171 hbuss 162
               {
1246 killagreg 163
                NeutralAccZ += 0.02;
164
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
165
               }
166
             }
1171 hbuss 167
             else if(AdWertAccHoch < -1)
168
             {
1246 killagreg 169
              if(NeutralAccZ > 550)
1171 hbuss 170
                {
171
                 NeutralAccZ-= 0.02;
1246 killagreg 172
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;
173
                }
174
             }
1171 hbuss 175
            messanzahl_AccHoch = 1;
176
            Aktuell_az = ADC;
177
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
178
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
179
                kanal = AD_DRUCK;
180
            break;
181
   // "case 8:" fehlt hier absichtlich
182
        case 10:
183
            nick1 += ADC;
184
            kanal = AD_ROLL;
185
            break;
186
        case 11:
187
            roll1 += ADC;
188
                    kanal = AD_GIER;
189
            break;
190
        case 12:
191
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 1) / 2;
1246 killagreg 192
            else
1171 hbuss 193
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1);
194
                        else                                       AdWertGier = (ADC + gier1);
195
            kanal = AD_ACC_Y;
196
            break;
197
        case 13:
198
            Aktuell_ay = NeutralAccY - ADC;
199
            AdWertAccRoll = (Aktuell_ay + accy);
200
            kanal = AD_ACC_X;
201
            break;
202
        case 14:
203
            Aktuell_ax = ADC - NeutralAccX;
204
            AdWertAccNick =  (Aktuell_ax + accx);
205
            kanal = AD_NICK;
206
            break;
207
        case 15:
208
            nick1 += ADC;
1173 hbuss 209
            if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4;
210
            AdWertNick = nick1 / 8;
211
            nick_filter = (nick_filter + nick1) / 2;
212
            HiResNick = nick_filter - AdNeutralNick;
1171 hbuss 213
            AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
214
            kanal = AD_ROLL;
215
            break;
216
        case 16:
217
            roll1 += ADC;
1173 hbuss 218
            if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4;
219
            AdWertRoll = roll1 / 8;
220
            roll_filter = (roll_filter + roll1) / 2;
221
            HiResRoll = roll_filter - AdNeutralRoll;
1171 hbuss 222
            AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
223
                kanal = AD_DRUCK;
224
            break;
225
        case 17:
226
            state = 0;
227
                        AdReady = 1;
228
            ZaehlMessungen++;
229
            // "break" fehlt hier absichtlich
230
        case 9:
1253 killagreg 231
                MessLuftdruck = ADC;
232
            tmpLuftdruck += MessLuftdruck;
233
            if(++messanzahl_Druck >= 18)
234
            {
1266 killagreg 235
                                Luftdruck = (7 * Luftdruck + tmpLuftdruck - (18 * 523) * (long)ExpandBaro + 4) / 8;  // -523.19 counts per 10 counts offset step
1253 killagreg 236
                                HoehenWert = StartLuftdruck - Luftdruck;
1272 hbuss 237
                                SummenHoehe -= SummenHoehe/SM_FILTER;
1253 killagreg 238
                                SummenHoehe += HoehenWert;
1272 hbuss 239
                                VarioMeter = (15 * VarioMeter + 8 * (int)(HoehenWert - SummenHoehe/SM_FILTER))/16;
240
                tmpLuftdruck /= 2;
241
                messanzahl_Druck = 18/2;
1253 killagreg 242
            }
1171 hbuss 243
            kanal = AD_NICK;
244
            break;
1246 killagreg 245
        default:
1171 hbuss 246
            kanal = 0; state = 0; kanal = AD_NICK;
247
            break;
1246 killagreg 248
        }
1171 hbuss 249
    ADMUX = kanal;
250
    if(state != 0) ANALOG_ON;
1320 hbuss 251
 
1171 hbuss 252
}
253
 
254
 
255
 
256
/*
257
//#######################################################################################
258
//
259
SIGNAL(SIG_ADC)
260
//#######################################################################################
261
{
262
    static unsigned char kanal=0,state = 0;
1166 hbuss 263
    static signed int gier1, roll1, nick1;
1246 killagreg 264
        static signed long nick_filter, roll_filter;
1166 hbuss 265
        static signed int accy, accx;
266
    switch(state++)
267
        {
268
        case 0:
269
            nick1 = ADC;
270
            kanal = AD_ROLL;
271
            break;
272
        case 1:
273
            roll1 = ADC;
274
                    kanal = AD_GIER;
275
            break;
276
        case 2:
277
            gier1 = ADC;
278
            kanal = AD_ACC_Y;
279
            break;
280
        case 3:
281
            Aktuell_ay = NeutralAccY - ADC;
282
            accy = Aktuell_ay;
283
            kanal = AD_NICK;
284
            break;
285
        case 4:
286
            nick1 += ADC;
287
            kanal = AD_ROLL;
288
            break;
289
        case 5:
290
            roll1 += ADC;
291
                    kanal = AD_ACC_Z;
292
            break;
293
       case 6:
294
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
1246 killagreg 295
            if(AdWertAccHoch > 1)
1166 hbuss 296
             {
1246 killagreg 297
              if(NeutralAccZ < 750)
1166 hbuss 298
               {
1246 killagreg 299
                NeutralAccZ += 0.02;
300
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
301
               }
302
             }
1166 hbuss 303
             else if(AdWertAccHoch < -1)
304
             {
1246 killagreg 305
              if(NeutralAccZ > 550)
1166 hbuss 306
                {
307
                 NeutralAccZ-= 0.02;
1246 killagreg 308
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;
309
                }
310
             }
1166 hbuss 311
            messanzahl_AccHoch = 1;
312
            Aktuell_az = ADC;
313
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
314
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
315
            kanal = AD_NICK;
316
            break;
317
        case 7:
318
            nick1 += ADC;
319
            kanal = AD_ROLL;
320
            break;
321
        case 8:
322
            roll1 += ADC;
323
                    kanal = AD_ACC_X;
324
            break;
325
        case 9:
326
            Aktuell_ax = ADC - NeutralAccX;
327
            accx =  Aktuell_ax;
328
                    kanal = AD_GIER;
329
            break;
330
        case 10:
331
            gier1 += ADC;
332
            kanal = AD_NICK;
333
            break;
334
        case 11:
335
            nick1 += ADC;
336
            kanal = AD_ROLL;
337
            break;
338
        case 12:
339
            roll1 += ADC;
340
            kanal = AD_UBAT;
341
            break;
342
        case 13:
343
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
344
            kanal = AD_ACC_Y;
345
            break;
346
        case 14:
347
            Aktuell_ay = NeutralAccY - ADC;
348
            accy += Aktuell_ay;
349
            kanal = AD_NICK;
350
            break;
351
        case 15:
352
            nick1 += ADC;
353
            kanal = AD_ROLL;
354
            break;
355
        case 16:
356
            roll1 += ADC;
357
            kanal = AD_ACC_X;
358
            break;
359
        case 17:
360
            Aktuell_ax = ADC - NeutralAccX;
361
            accx += Aktuell_ax;
362
                    kanal = AD_NICK;
363
            break;
364
        case 18:
365
            nick1 += ADC;
366
            kanal = AD_ROLL;
367
            break;
368
        case 19:
369
            roll1 += ADC;
370
                    kanal = AD_GIER;
371
            break;
372
        case 20:
373
            gier1 += ADC;
374
            kanal = AD_ACC_Y;
375
            break;
376
        case 21:
377
            Aktuell_ay = NeutralAccY - ADC;
378
            accy += Aktuell_ay;
379
            kanal = AD_NICK;
380
            break;
381
        case 22:
382
            nick1 += ADC;
383
            kanal = AD_ROLL;
384
            break;
385
        case 23:
386
            roll1 += ADC;
387
                kanal = AD_DRUCK;
388
            break;
389
        case 24:
390
            tmpLuftdruck += ADC;
1246 killagreg 391
            if(++messanzahl_Druck >= 5)
1166 hbuss 392
                {
393
                MessLuftdruck = ADC;
394
                messanzahl_Druck = 0;
1167 hbuss 395
                                HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8;  // D-Anteil = neuerWert - AlterWert
1166 hbuss 396
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
397
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
398
                tmpLuftdruck = 0;
1246 killagreg 399
                }
1166 hbuss 400
            kanal = AD_NICK;
401
            break;
402
        case 25:
403
            nick1 += ADC;
404
            kanal = AD_ROLL;
405
            break;
406
        case 26:
407
            roll1 += ADC;
408
                    kanal = AD_ACC_X;
409
            break;
410
        case 27:
411
            Aktuell_ax = ADC - NeutralAccX;
412
            accx +=  Aktuell_ax;
413
                    kanal = AD_GIER;
414
            break;
415
        case 28:
416
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 2) / 4;
1246 killagreg 417
            else
1166 hbuss 418
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
419
                        else                                       AdWertGier = (ADC + gier1 + 1) / 2;
420
            kanal = AD_NICK;
421
            break;
422
        case 29:
423
            nick1 += ADC;
424
            kanal = AD_ROLL;
425
            break;
426
        case 30:
427
            roll1 += ADC;
428
            kanal = AD_ACC_Y;
429
            break;
430
        case 31:
431
            Aktuell_ay = NeutralAccY - ADC;
432
            AdWertAccRoll = (Aktuell_ay + accy);
433
            kanal = AD_NICK;
434
            break;
435
        case 32:
436
            AdWertNick = (ADC + nick1 + 3) / 5;
437
            nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
438
            if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
439
            HiResNick = nick_filter - 20 * AdNeutralNick;
440
            AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
1246 killagreg 441
            DebugOut.Analog[21] = AdWertNickFilter / 4;
1166 hbuss 442
            kanal = AD_ROLL;
443
            break;
444
        case 33:
445
            AdWertRoll = (ADC + roll1 + 3) / 5;
446
            roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
447
            if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
448
            HiResRoll = roll_filter - 20 * AdNeutralRoll;
449
            AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
450
            DebugOut.Analog[22] = AdWertRollFilter / 4;
451
            kanal = AD_ACC_X;
452
            break;
453
        case 34:
454
            Aktuell_ax = ADC - NeutralAccX;
455
            AdWertAccNick =  (Aktuell_ax + accx);
456
                    kanal = AD_NICK;
457
            state = 0;
458
                        AdReady = 1;
459
            ZaehlMessungen++;
460
            break;
1246 killagreg 461
        default:
1166 hbuss 462
            kanal = 0;
463
            state = 0;
464
            break;
1246 killagreg 465
        }
1166 hbuss 466
    ADMUX = kanal;
467
    if(state != 0) ANALOG_ON;
468
}
1171 hbuss 469
*/