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;
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;
1168 hbuss 25
volatile unsigned char AdReady = 1;
2008 holgerb 26
 
1 ingob 27
//#######################################################################################
28
void ADC_Init(void)
29
//#######################################################################################
1246 killagreg 30
{
1 ingob 31
    ADMUX = 0;//Referenz ist extern
1155 hbuss 32
    ANALOG_ON;
1 ingob 33
}
34
 
1352 hbuss 35
#define DESIRED_H_ADC 800
1322 hbuss 36
 
1 ingob 37
void SucheLuftruckOffset(void)
38
{
39
 unsigned int off;
1036 hbuss 40
 ExpandBaro = 0;
1726 holgerb 41
 
42
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
43
 {
44
  unsigned char off2;
45
  OCR0A = 150;
46
  off2 = GetParamByte(PID_PRESSURE_OFFSET);
47
  if(off2 < 230) off2 += 10;
48
  OCR0B = off2;
49
  Delay_ms_Mess(100);
50
  if(MessLuftdruck > DESIRED_H_ADC) off2 = 240;
51
  for(; off2 >= 5; off2 -= 5)
52
   {
53
   OCR0B = off2;
54
   Delay_ms_Mess(50);
55
   printf("*");
56
   if(MessLuftdruck > DESIRED_H_ADC) break;
57
   }
58
   SetParamByte(PID_PRESSURE_OFFSET, off2);
59
  if(off2 >= 15) off = 140; else off = 0;
60
  for(; off < 250;off++)
61
   {
62
   OCR0A = off;
63
   Delay_ms_Mess(50);
64
   printf(".");
65
   if(MessLuftdruck < DESIRED_H_ADC) break;
66
   }
67
   DruckOffsetSetting = off;
68
 }
1765 killagreg 69
#else
1726 holgerb 70
  off = GetParamByte(PID_PRESSURE_OFFSET);
71
  if(off > 20) off -= 10;
1 ingob 72
  OCR0A = off;
1726 holgerb 73
  Delay_ms_Mess(100);
74
  if(MessLuftdruck < DESIRED_H_ADC) off = 0;
75
  for(; off < 250;off++)
76
   {
77
   OCR0A = off;
78
   Delay_ms_Mess(50);
79
   printf(".");
80
   if(MessLuftdruck < DESIRED_H_ADC) break;
81
   }
82
   DruckOffsetSetting = off;
83
   SetParamByte(PID_PRESSURE_OFFSET, off);
84
#endif
1765 killagreg 85
 if((EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG) && (DruckOffsetSetting < 10 || DruckOffsetSetting >= 245)) VersionInfo.HardwareError[0] |= FC_ERROR0_PRESSURE;
1638 holgerb 86
 OCR0A = off;
380 hbuss 87
 Delay_ms_Mess(300);
1 ingob 88
}
89
 
1726 holgerb 90
 
918 hbuss 91
void SucheGyroOffset(void)
92
{
93
 unsigned char i, ready = 0;
1219 hbuss 94
 int timeout;
95
 timeout = SetDelay(2000);
918 hbuss 96
 for(i=140; i != 0; i--)
97
  {
921 hbuss 98
   if(ready == 3 && i > 10) i = 9;
918 hbuss 99
   ready = 0;
100
   if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
101
   if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
102
   if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
1662 killagreg 103
   I2C_Start(TWI_STATE_GYRO_OFFSET_TX);
1765 killagreg 104
   if(AnalogOffsetNick < 10)  { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_NICK; AnalogOffsetNick = 245;};
105
   if(AnalogOffsetRoll < 10)  { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_ROLL; AnalogOffsetRoll = 245;};
106
   if(AnalogOffsetGier < 10)  { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW;  AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { VersionInfo.HardwareError[0] |= FC_ERROR0_GYRO_YAW;  AnalogOffsetGier = 245;};
1219 hbuss 107
   while(twi_state) if(CheckDelay(timeout)) {printf("\n\r DAC or I2C ERROR! Check I2C, 3Vref, DAC and BL-Ctrl"); break;}
1253 killagreg 108
   AdReady = 0;
918 hbuss 109
   ANALOG_ON;
1253 killagreg 110
   while(!AdReady);
1246 killagreg 111
   if(i<10) Delay_ms_Mess(10);
918 hbuss 112
  }
1246 killagreg 113
   Delay_ms_Mess(70);
114
}
1 ingob 115
 
1171 hbuss 116
/*
117
 
118
1  r
1246 killagreg 119
2     g
1171 hbuss 120
3     y
121
4     x
122
5  n
123
6  r
124
7     u
125
8     z
126
9     L
1246 killagreg 127
10 n
1171 hbuss 128
11 r
129
12    g
130
13    y
131
14    x
132
15 n
133
16 r
134
17    L
135
*/
136
 
1 ingob 137
//#######################################################################################
138
//
1561 killagreg 139
ISR(ADC_vect)
1 ingob 140
//#######################################################################################
141
{
142
    static unsigned char kanal=0,state = 0;
1687 holgerb 143
        static signed char subcount = 0;
1246 killagreg 144
    static signed int gier1, roll1, nick1, nick_filter, roll_filter;
1687 holgerb 145
        static signed int accy, accx;
1253 killagreg 146
        static long tmpLuftdruck = 0;
147
        static char messanzahl_Druck = 0;
1171 hbuss 148
    switch(state++)
149
        {
150
        case 0:
151
            nick1 = ADC;
152
            kanal = AD_ROLL;
153
            break;
154
        case 1:
155
            roll1 = ADC;
156
                    kanal = AD_GIER;
157
            break;
158
        case 2:
159
            gier1 = ADC;
160
            kanal = AD_ACC_Y;
161
            break;
162
        case 3:
163
            Aktuell_ay = NeutralAccY - ADC;
164
            accy = Aktuell_ay;
165
                    kanal = AD_ACC_X;
166
            break;
167
        case 4:
168
            Aktuell_ax = ADC - NeutralAccX;
169
            accx =  Aktuell_ax;
170
            kanal = AD_NICK;
171
            break;
172
        case 5:
173
            nick1 += ADC;
174
            kanal = AD_ROLL;
175
            break;
176
        case 6:
177
            roll1 += ADC;
178
            kanal = AD_UBAT;
179
            break;
180
        case 7:
1806 holgerb 181
#if (defined(__AVR_ATmega1284__) || defined(__AVR_ATmega1284P__))
182
            if(EE_Parameter.ExtraConfig & CFG_3_3V_REFERENCE) UBat = (3 * UBat + (11 * ADC) / 30) / 4; // there were some single FC2.1 with 3.3V reference
183
                        else  
184
#endif
185
                        UBat = (3 * UBat + ADC / 3) / 4;
1171 hbuss 186
                    kanal = AD_ACC_Z;
187
            break;
188
       case 8:
189
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
1246 killagreg 190
            if(AdWertAccHoch > 1)
1171 hbuss 191
             {
1246 killagreg 192
              if(NeutralAccZ < 750)
1171 hbuss 193
               {
1695 holgerb 194
                subcount += 5;
1701 holgerb 195
                if(modell_fliegt < 500) subcount += 10;
1639 holgerb 196
               }
1687 holgerb 197
              if(subcount > 100) { NeutralAccZ++; subcount -= 100;}
1639 holgerb 198
             }
199
             else if(AdWertAccHoch < -1)
200
             {
201
              if(NeutralAccZ > 550)
202
                {
1695 holgerb 203
                 subcount -= 5;
1701 holgerb 204
                 if(modell_fliegt < 500) subcount -= 10;
1687 holgerb 205
                 if(subcount < -100) { NeutralAccZ--; subcount += 100;}
1639 holgerb 206
                }
207
             }
2008 holgerb 208
//            messanzahl_AccHoch = 1;
1171 hbuss 209
            Aktuell_az = ADC;
210
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
211
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
212
                kanal = AD_DRUCK;
213
            break;
1701 holgerb 214
   // "case 9:" fehlt hier absichtlich
1171 hbuss 215
        case 10:
216
            nick1 += ADC;
217
            kanal = AD_ROLL;
218
            break;
219
        case 11:
220
            roll1 += ADC;
221
                    kanal = AD_GIER;
222
            break;
223
        case 12:
224
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 1) / 2;
1246 killagreg 225
            else
1660 holgerb 226
            if(PlatinenVersion >= 20)  AdWertGier = 2047 - (ADC + gier1);
1171 hbuss 227
                        else                                       AdWertGier = (ADC + gier1);
228
            kanal = AD_ACC_Y;
229
            break;
230
        case 13:
231
            Aktuell_ay = NeutralAccY - ADC;
232
            AdWertAccRoll = (Aktuell_ay + accy);
233
            kanal = AD_ACC_X;
234
            break;
235
        case 14:
236
            Aktuell_ax = ADC - NeutralAccX;
237
            AdWertAccNick =  (Aktuell_ax + accx);
238
            kanal = AD_NICK;
239
            break;
240
        case 15:
241
            nick1 += ADC;
1173 hbuss 242
            if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4;
243
            AdWertNick = nick1 / 8;
244
            nick_filter = (nick_filter + nick1) / 2;
245
            HiResNick = nick_filter - AdNeutralNick;
1171 hbuss 246
            AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
247
            kanal = AD_ROLL;
248
            break;
249
        case 16:
250
            roll1 += ADC;
1173 hbuss 251
            if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4;
252
            AdWertRoll = roll1 / 8;
253
            roll_filter = (roll_filter + roll1) / 2;
254
            HiResRoll = roll_filter - AdNeutralRoll;
1171 hbuss 255
            AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
256
                kanal = AD_DRUCK;
257
            break;
258
        case 17:
259
            state = 0;
260
                        AdReady = 1;
261
            ZaehlMessungen++;
262
            // "break" fehlt hier absichtlich
263
        case 9:
1253 killagreg 264
                MessLuftdruck = ADC;
265
            tmpLuftdruck += MessLuftdruck;
2009 holgerb 266
            if(++messanzahl_Druck >= 16) // war bis 0.86 "18"
1253 killagreg 267
            {
1944 holgerb 268
                            signed int tmp;
2009 holgerb 269
                                Luftdruck = (7 * Luftdruck + tmpLuftdruck - (16 * 523) * (long)ExpandBaro + 4) / 8;  // -523.19 counts per 10 counts offset step
1253 killagreg 270
                                HoehenWert = StartLuftdruck - Luftdruck;
1272 hbuss 271
                                SummenHoehe -= SummenHoehe/SM_FILTER;
1253 killagreg 272
                                SummenHoehe += HoehenWert;
1944 holgerb 273
                                tmp = (HoehenWert - SummenHoehe/SM_FILTER);
1982 holgerb 274
                                if(tmp > 1024) tmp = 1024;      else if(tmp < -1024) tmp = -1024;
275
                if(abs(VarioMeter) > 700) VarioMeter = (15 * VarioMeter + 8 * tmp)/16;
1944 holgerb 276
                                else VarioMeter = (31 * VarioMeter + 8 * tmp)/32;
1272 hbuss 277
                tmpLuftdruck /= 2;
2009 holgerb 278
                messanzahl_Druck = 16/2;
1253 killagreg 279
            }
1171 hbuss 280
            kanal = AD_NICK;
281
            break;
1246 killagreg 282
        default:
1171 hbuss 283
            kanal = 0; state = 0; kanal = AD_NICK;
284
            break;
1246 killagreg 285
        }
1171 hbuss 286
    ADMUX = kanal;
287
    if(state != 0) ANALOG_ON;
288
}
289