Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

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