Subversion Repositories FlightCtrl

Rev

Rev 1226 | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

Rev 1226 Rev 1286
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
2
// + Copyright (c) 04.2007 Holger Buss
3
// + only for non-profit use
3
// + only for non-profit use
4
// + www.MikroKopter.com
4
// + www.MikroKopter.com
5
// + see the File "License.txt" for further Informations
5
// + see the File "License.txt" for further Informations
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
 
7
 
8
#include "main.h"
8
#include "main.h"
9
volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
9
volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
10
volatile int  AdWertNickFilter = 0, AdWertRollFilter = 0, AdWertGierFilter = 0;
10
volatile int  AdWertNickFilter = 0, AdWertRollFilter = 0, AdWertGierFilter = 0;
11
volatile int  HiResNick = 2500, HiResRoll = 2500;
11
volatile int  HiResNick = 2500, HiResRoll = 2500;
12
volatile int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
12
volatile int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
13
volatile int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
13
volatile int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
14
volatile char messanzahl_AccHoch = 0;
14
volatile char messanzahl_AccHoch = 0;
15
volatile long Luftdruck = 32000;
15
volatile long Luftdruck = 32000;
16
volatile int  StartLuftdruck;
16
volatile int  StartLuftdruck;
17
volatile unsigned int  MessLuftdruck = 1023;
17
volatile unsigned int  MessLuftdruck = 1023;
18
unsigned char DruckOffsetSetting;
18
unsigned char DruckOffsetSetting;
19
signed char ExpandBaro = 0;
19
signed char ExpandBaro = 0;
20
volatile int HoeheD = 0;
20
volatile int HoeheD = 0;
21
volatile char messanzahl_Druck;
21
volatile char messanzahl_Druck;
22
volatile int  tmpLuftdruck;
22
volatile int  tmpLuftdruck;
23
volatile unsigned int ZaehlMessungen = 0;
23
volatile unsigned int ZaehlMessungen = 0;
24
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
24
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
25
unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
25
unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
26
volatile unsigned char AdReady = 1;
26
volatile unsigned char AdReady = 1;
27
//#######################################################################################
27
//#######################################################################################
28
//
28
//
29
void ADC_Init(void)
29
void ADC_Init(void)
30
//#######################################################################################
30
//#######################################################################################
31
{
31
{
32
    ADMUX = 0;//Referenz ist extern
32
    ADMUX = 0;//Referenz ist extern
33
    ANALOG_ON;
33
    ANALOG_ON;
34
}
34
}
35
 
35
 
36
void SucheLuftruckOffset(void)
36
void SucheLuftruckOffset(void)
37
{
37
{
38
 unsigned int off;
38
 unsigned int off;
39
 off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
39
 off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
40
 if(off > 20) off -= 10;
40
 if(off > 20) off -= 10;
41
 OCR0A = off;
41
 OCR0A = off;
42
 ExpandBaro = 0;
42
 ExpandBaro = 0;
43
 Delay_ms_Mess(100);
43
 Delay_ms_Mess(100);
44
 if(MessLuftdruck < 850) off = 0;
44
 if(MessLuftdruck < 850) off = 0;
45
 for(; off < 250;off++)
45
 for(; off < 250;off++)
46
  {
46
  {
47
  OCR0A = off;
47
  OCR0A = off;
48
  Delay_ms_Mess(50);
48
  Delay_ms_Mess(50);
49
  printf(".");  
49
  printf(".");  
50
  if(MessLuftdruck < 850) break;
50
  if(MessLuftdruck < 850) break;
51
  }
51
  }
52
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
52
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
53
 DruckOffsetSetting = off;
53
 DruckOffsetSetting = off;
54
 Delay_ms_Mess(300);
54
 Delay_ms_Mess(300);
55
}
55
}
56
 
56
 
57
void SucheGyroOffset(void)
57
void SucheGyroOffset(void)
58
{
58
{
59
 unsigned char i, ready = 0;
59
 unsigned char i, ready = 0;
60
 int timeout;
60
 int timeout;
61
 GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
61
 GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
62
 timeout = SetDelay(2000);
62
 timeout = SetDelay(2000);
63
 for(i=140; i != 0; i--)
63
 for(i=140; i != 0; i--)
64
  {
64
  {
65
   if(ready == 3 && i > 10) i = 9;
65
   if(ready == 3 && i > 10) i = 9;
66
   ready = 0;
66
   ready = 0;
67
   if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
67
   if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
68
   if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
68
   if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
69
   if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
69
   if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
70
   twi_state = 8;  
70
   twi_state = 8;  
71
   i2c_start();
71
   i2c_start();
72
   if(AnalogOffsetNick < 10)  { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};
72
   if(AnalogOffsetNick < 10)  { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};
73
   if(AnalogOffsetRoll < 10)  { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};
73
   if(AnalogOffsetRoll < 10)  { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};
74
   if(AnalogOffsetGier < 10)  { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
74
   if(AnalogOffsetGier < 10)  { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
75
   while(twi_state) if(CheckDelay(timeout)) {printf("\n\r DAC or I2C ERROR! Check I2C, 3Vref, DAC and BL-Ctrl"); break;}
75
   while(twi_state) if(CheckDelay(timeout)) {printf("\n\r DAC or I2C ERROR! Check I2C, 3Vref, DAC and BL-Ctrl"); break;}
76
   messanzahl_Druck = 0;
76
   messanzahl_Druck = 0;
77
   ANALOG_ON;
77
   ANALOG_ON;
78
   while(messanzahl_Druck == 0);
78
   while(messanzahl_Druck == 0);
79
   if(i<10) Delay_ms_Mess(10);  
79
   if(i<10) Delay_ms_Mess(10);  
80
  }
80
  }
81
   Delay_ms_Mess(70);  
81
   Delay_ms_Mess(70);  
82
}
82
}
83
 
83
 
84
/*
84
/* // MartinR: so war es
85
0  n
85
0  n
86
1  r
86
1  r
87
2     g
87
2     g
88
3     y
88
3     y
89
4     x
89
4     x
90
5  n
90
5  n
91
6  r
91
6  r
92
7     u
92
7     u
93
8     z
93
8     z
94
9     L
94
9     L
95
10 n  
95
10 n  
96
11 r
96
11 r
97
12    g
97
12    g
98
13    y
98
13    y
99
14    x
99
14    x
100
15 n
100
15 n
101
16 r
101
16 r
102
17    L
102
17    L
103
*/
103
*/
-
 
104
 
-
 
105
/*  MartinR : geändert
-
 
106
0  n
-
 
107
1  r
-
 
108
2     g
-
 
109
3     L
-
 
110
4  n
-
 
111
5  r
-
 
112
6         y
-
 
113
7     x
-
 
114
8  n  
-
 
115
9  r  
-
 
116
10    g
-
 
117
11    L
-
 
118
12 n  
-
 
119
13 r  
-
 
120
14    u
-
 
121
15    z
-
 
122
*/
104
 
123
 
105
//#######################################################################################
124
//#######################################################################################
106
//
125
//
107
SIGNAL(SIG_ADC)
126
SIGNAL(SIG_ADC)
108
//#######################################################################################
127
//#######################################################################################
109
{
128
{
110
    static unsigned char kanal=0,state = 0;
129
    static unsigned char kanal=0,state = 0;
111
    static signed int gier1, roll1, nick1, nick_filter, roll_filter;   
130
    static signed int gier1, roll1, nick1, nick_filter, roll_filter;   
-
 
131
        static signed int roll_alt1, roll_alt2, roll_alt3, nick_alt1, nick_alt2, nick_alt3;     // MartinR: 4x Mittelung
-
 
132
        //static signed int roll_alt1, nick_alt1;       // MartinR: 2x Mittelung
112
        static signed int accy, accx;
133
        static signed int accy, accx;
-
 
134
       
-
 
135
  /*    // MartinR : so war es Anfang  
113
    switch(state++)
136
    switch(state++)
114
        {
137
        {
115
        case 0:
138
        case 0:
116
            nick1 = ADC;
139
            nick1 = ADC;
117
            kanal = AD_ROLL;
140
            kanal = AD_ROLL;
118
            break;
141
            break;
119
        case 1:
142
        case 1:
120
            roll1 = ADC;
143
            roll1 = ADC;
121
                    kanal = AD_GIER;
144
                    kanal = AD_GIER;
122
            break;
145
            break;
123
        case 2:
146
        case 2:
124
            gier1 = ADC;
147
            gier1 = ADC;
125
            kanal = AD_ACC_Y;
148
            kanal = AD_ACC_Y;
126
            break;
149
            break;
127
        case 3:
150
        case 3:
128
            Aktuell_ay = NeutralAccY - ADC;
151
            Aktuell_ay = NeutralAccY - ADC;
129
            accy = Aktuell_ay;
152
            accy = Aktuell_ay;
130
                    kanal = AD_ACC_X;
153
                    kanal = AD_ACC_X;
131
            break;
154
            break;
132
        case 4:
155
        case 4:
133
            Aktuell_ax = ADC - NeutralAccX;
156
            Aktuell_ax = ADC - NeutralAccX;
134
            accx =  Aktuell_ax;
157
            accx =  Aktuell_ax;
135
            kanal = AD_NICK;
158
            kanal = AD_NICK;
136
            break;
159
            break;
137
        case 5:
160
        case 5:
138
            nick1 += ADC;
161
            nick1 += ADC;
139
            kanal = AD_ROLL;
162
            kanal = AD_ROLL;
140
            break;
163
            break;
141
        case 6:
164
        case 6:
142
            roll1 += ADC;
165
            roll1 += ADC;
143
            kanal = AD_UBAT;
166
            kanal = AD_UBAT;
144
            break;
167
            break;
145
        case 7:
168
        case 7:
146
            UBat = (3 * UBat + ADC / 3) / 4;
169
            UBat = (3 * UBat + ADC / 3) / 4;
147
                    kanal = AD_ACC_Z;
170
                    kanal = AD_ACC_Z;
148
            break;
171
            break;
149
       case 8:
172
       case 8:
150
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
173
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
151
            if(AdWertAccHoch > 1)
174
            if(AdWertAccHoch > 1)
152
             {
175
             {
153
              if(NeutralAccZ < 750)
176
              if(NeutralAccZ < 750)
154
               {
177
               {
155
                NeutralAccZ += 0.02;
178
                NeutralAccZ += 0.02;
156
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
179
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
157
               }
180
               }
158
             }  
181
             }  
159
             else if(AdWertAccHoch < -1)
182
             else if(AdWertAccHoch < -1)
160
             {
183
             {
161
              if(NeutralAccZ > 550)
184
              if(NeutralAccZ > 550)
162
                {
185
                {
163
                 NeutralAccZ-= 0.02;
186
                 NeutralAccZ-= 0.02;
164
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
187
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
165
                }
188
                }
166
             }
189
             }
167
            messanzahl_AccHoch = 1;
190
            messanzahl_AccHoch = 1;
168
            Aktuell_az = ADC;
191
            Aktuell_az = ADC;
169
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
192
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
170
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
193
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
171
                kanal = AD_DRUCK;
194
                kanal = AD_DRUCK;
172
            break;
195
            break;
173
   // "case 8:" fehlt hier absichtlich
196
   // "case 8:" fehlt hier absichtlich
174
        case 10:
197
        case 10:
175
            nick1 += ADC;
198
            nick1 += ADC;
176
            kanal = AD_ROLL;
199
            kanal = AD_ROLL;
177
            break;
200
            break;
178
        case 11:
201
        case 11:
179
            roll1 += ADC;
202
            roll1 += ADC;
180
                    kanal = AD_GIER;
203
                    kanal = AD_GIER;
181
            break;
204
            break;
182
        case 12:
205
        case 12:
183
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 1) / 2;
206
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 1) / 2;
184
            else
207
            else
185
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1);
208
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1);
186
                        else                                       AdWertGier = (ADC + gier1);
209
                        else                                       AdWertGier = (ADC + gier1);
187
            kanal = AD_ACC_Y;
210
            kanal = AD_ACC_Y;
188
            break;
211
            break;
189
        case 13:
212
        case 13:
190
            Aktuell_ay = NeutralAccY - ADC;
213
            Aktuell_ay = NeutralAccY - ADC;
191
            AdWertAccRoll = (Aktuell_ay + accy);
214
            AdWertAccRoll = (Aktuell_ay + accy);
192
            kanal = AD_ACC_X;
215
            kanal = AD_ACC_X;
193
            break;
216
            break;
194
        case 14:
217
        case 14:
195
            Aktuell_ax = ADC - NeutralAccX;
218
            Aktuell_ax = ADC - NeutralAccX;
196
            AdWertAccNick =  (Aktuell_ax + accx);
219
            AdWertAccNick =  (Aktuell_ax + accx);
197
            kanal = AD_NICK;
220
            kanal = AD_NICK;
198
            break;
221
            break;
199
        case 15:
222
        case 15:
200
            nick1 += ADC;
223
            nick1 += ADC;
201
            if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4;
224
            if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4;
202
            AdWertNick = nick1 / 8;
225
            AdWertNick = nick1 / 8;
203
            nick_filter = (nick_filter + nick1) / 2;
226
            nick_filter = (nick_filter + nick1) / 2;
204
            HiResNick = nick_filter - AdNeutralNick;
227
            HiResNick = nick_filter - AdNeutralNick;
205
            AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
228
            AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2;
206
            kanal = AD_ROLL;
229
            kanal = AD_ROLL;
207
            break;
230
            break;
208
        case 16:
231
        case 16:
209
            roll1 += ADC;
232
            roll1 += ADC;
210
            if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4;
233
            if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4;
211
            AdWertRoll = roll1 / 8;
234
            AdWertRoll = roll1 / 8;
212
            roll_filter = (roll_filter + roll1) / 2;
235
            roll_filter = (roll_filter + roll1) / 2;
213
            HiResRoll = roll_filter - AdNeutralRoll;
236
            HiResRoll = roll_filter - AdNeutralRoll;
214
            AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
237
            AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;
215
                kanal = AD_DRUCK;
238
                kanal = AD_DRUCK;
216
            break;
239
            break;
217
        case 17:
240
        case 17:
218
            state = 0;
241
            state = 0;
219
                        AdReady = 1;
242
                        AdReady = 1;
220
            ZaehlMessungen++;
243
            ZaehlMessungen++;
221
            // "break" fehlt hier absichtlich
244
            // "break" fehlt hier absichtlich
222
        case 9:
245
        case 9:
223
            tmpLuftdruck += ADC;
246
            tmpLuftdruck += ADC;
224
            if(++messanzahl_Druck >= 5)
247
            if(++messanzahl_Druck >= 5)
225
                {
248
                {
226
                                tmpLuftdruck /= 2;
249
                                tmpLuftdruck /= 2;
227
                MessLuftdruck = ADC;
250
                MessLuftdruck = ADC;
228
                messanzahl_Druck = 0;
251
                messanzahl_Druck = 0;
229
                                HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/32;  // D-Anteil = neuerWert - AlterWert
252
                                HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/32;  // D-Anteil = neuerWert - AlterWert
230
                Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
253
                Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
231
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
254
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
232
                tmpLuftdruck /= 2;
255
                tmpLuftdruck /= 2;
233
                }
256
                }
234
            kanal = AD_NICK;
257
            kanal = AD_NICK;
235
            break;
258
            break;
236
        default:  
259
        default:  
237
            kanal = 0; state = 0; kanal = AD_NICK;
260
            kanal = 0; state = 0; kanal = AD_NICK;
238
            break;
261
            break;
239
        }
262
        }
-
 
263
                */  // MartinR : so war es Ende
-
 
264
        // MartinR : geändert Anfang
-
 
265
       
-
 
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
                       
-
 
277
                case 2:
-
 
278
            gier1 = ADC;
-
 
279
            kanal = AD_DRUCK;
-
 
280
            break;     
-
 
281
                case 3:
-
 
282
                        tmpLuftdruck += ADC;
-
 
283
                        messanzahl_Druck ++;
-
 
284
            kanal = AD_NICK;
-
 
285
            break;     
-
 
286
                       
-
 
287
                case 4:
-
 
288
            nick1 += ADC;
-
 
289
            kanal = AD_ROLL;
-
 
290
            break;
-
 
291
        case 5:
-
 
292
            roll1 += ADC;
-
 
293
            kanal = AD_ACC_Y;
-
 
294
            break;             
-
 
295
                       
-
 
296
                case 6:
-
 
297
            Aktuell_ay = NeutralAccY - ADC;
-
 
298
                        AdWertAccRoll = (Aktuell_ay + accy);
-
 
299
                        accy = Aktuell_ay;
-
 
300
                    kanal = AD_ACC_X;
-
 
301
            break;     
-
 
302
                case 7:
-
 
303
            Aktuell_ax = ADC - NeutralAccX;
-
 
304
                        AdWertAccNick =  (Aktuell_ax + accx);
-
 
305
            accx =  Aktuell_ax;
-
 
306
            kanal = AD_NICK;
-
 
307
            break;
-
 
308
                       
-
 
309
                case 8:
-
 
310
            nick1 += ADC;
-
 
311
            kanal = AD_ROLL;
-
 
312
            break;
-
 
313
        case 9:
-
 
314
            roll1 += ADC;      
-
 
315
                        kanal = AD_GIER;
-
 
316
            break;     
-
 
317
                       
-
 
318
                case 10:
-
 
319
                        if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 1) / 2;
-
 
320
            else
-
 
321
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1);
-
 
322
                        else                                       AdWertGier = (ADC + gier1);
-
 
323
            kanal = AD_DRUCK;
-
 
324
            break;
-
 
325
                       
-
 
326
                case 11:               
-
 
327
                               
-
 
328
                        // MartinR: neu
-
 
329
                        tmpLuftdruck += ADC;
-
 
330
                        //MessLuftdruck = tmpLuftdruck / 2;
-
 
331
                        MessLuftdruck = ADC;
-
 
332
                       
-
 
333
                        if(++messanzahl_Druck >= 5)
-
 
334
                        {
-
 
335
                                tmpLuftdruck -= 2616L * ExpandBaro; // -523.19 counts per 10 counts offset step
-
 
336
                                tmpLuftdruck /= 2;
-
 
337
                messanzahl_Druck = 0;
-
 
338
                                //HoeheD = (31 * HoeheD + (int) Parameter_Luftdruck_D * (int)((int)(StartLuftdruck - tmpLuftdruck) - HoehenWert))/32;  // D-Anteil = neuerWert - AlterWert
-
 
339
                Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
-
 
340
                HoehenWert = StartLuftdruck - Luftdruck;
-
 
341
                //tmpLuftdruck /= 2;
-
 
342
                                tmpLuftdruck = 0;
-
 
343
                        }
-
 
344
                       
-
 
345
                        // Luftdruck = (tmpLuftdruck + 7 * Luftdruck + 4) / 8;
-
 
346
            // HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck; // MartinR: zu Höhenregler
-
 
347
                       
-
 
348
            kanal = AD_NICK;
-
 
349
            break;
-
 
350
                       
-
 
351
                case 12:
-
 
352
            nick1 += ADC;
-
 
353
                        //if(PlatinenVersion == 10) nick1 *= 2; else nick1 *= 4; // MartinR: so war es
-
 
354
                        //if(PlatinenVersion == 10) ; else nick1 *= 2; // MartinR: 2x Mittelung
-
 
355
                        if(PlatinenVersion == 10) nick1 /= 2; //else nick1 *= 1; // MartinR: 4x Mittelung
-
 
356
            //AdWertNick = nick1 / 8; // MartinR: so war es
-
 
357
                        //AdWertNick = nick1 / 4; // MartinR: 2x Mittelung
-
 
358
                        AdWertNick = nick1 / 2; // MartinR: 4x Mittelung
-
 
359
                       
-
 
360
            // nick_filter = (nick_filter + nick1) / 2; // MartinR: so war es
-
 
361
                        //nick_filter = (nick_alt1 + nick1); // MartinR: anderer Nick-Filter: 2x Mittelung 
-
 
362
                        nick_filter = (nick_alt3 +nick_alt2 +nick_alt1 + nick1); // MartinR: anderer Nick-Filter: 4x Mittelung
-
 
363
                        nick_alt3 = nick_alt2; // MartinR: anderer Nick-Filter
-
 
364
                        nick_alt2 = nick_alt1; // MartinR: anderer Nick-Filter
-
 
365
                        nick_alt1 = nick1; // MartinR: anderer Nick-Filter
-
 
366
                       
-
 
367
            HiResNick = nick_filter - AdNeutralNick;
-
 
368
            //AdWertNickFilter = (AdWertNickFilter + HiResNick) / 2; // MartinR: so war es
-
 
369
                        AdWertNickFilter = HiResNick; // MartinR: Filter deaktiviert
-
 
370
            kanal = AD_ROLL;
-
 
371
                       
-
 
372
            break;
-
 
373
        case 13:
-
 
374
            roll1 += ADC;      
-
 
375
                        // if(PlatinenVersion == 10) roll1 *= 2; else roll1 *= 4; // MartinR: so war es
-
 
376
                        //if(PlatinenVersion == 10) ; else roll1 *= 2; // MartinR: 2x Mittelung
-
 
377
                        if(PlatinenVersion == 10) roll1 /= 2; //else roll1 *= 1; // MartinR: 4x Mittelung
-
 
378
            // AdWertRoll = roll1 / 8; // MartinR: so war es
-
 
379
                        //AdWertRoll = roll1 / 4; // MartinR: 2x Mittelung
-
 
380
                        AdWertRoll = roll1 / 2; // MartinR: 4x Mittelung
-
 
381
                       
-
 
382
            //roll_filter = (roll_filter + roll1) / 2; // MartinR: so war es
-
 
383
                        //roll_filter = (roll_alt1 + roll1); // MartinR: anderer Nick-Filter: 2x Mittelung
-
 
384
                        roll_filter = (roll_alt3 + roll_alt2 + roll_alt1 + roll1); // MartinR: anderer Nick-Filter: 4x Mittelung
-
 
385
                        roll_alt3 = roll_alt2; // MartinR: anderer Nick-Filter
-
 
386
                        roll_alt2 = roll_alt1; // MartinR: anderer Nick-Filter
-
 
387
                        roll_alt1 = roll1; // MartinR: anderer Nick-Filter
-
 
388
                               
-
 
389
            HiResRoll = roll_filter - AdNeutralRoll;
-
 
390
            //AdWertRollFilter = (AdWertRollFilter + HiResRoll) / 2;  // MartinR: so war es
-
 
391
                        AdWertRollFilter = HiResRoll; // MartinR: Filter deaktiviert
-
 
392
                       
-
 
393
                        kanal = AD_UBAT;
-
 
394
                       
-
 
395
            break;     
-
 
396
                       
-
 
397
                case 14:
-
 
398
            UBat = (3 * UBat + ADC / 3) / 4;
-
 
399
                    kanal = AD_ACC_Z;
-
 
400
            break;
-
 
401
                       
-
 
402
                case 15:       
-
 
403
                        // AdWertAccHoch =  (signed int) ADC - NeutralAccZ; // MartinR : so war es
-
 
404
                        //AdWertAccHoch =  ADC - NeutralAccZ; // MartinR: modifiziert für Tests
-
 
405
                        /* // MartinR: deaktiviert
-
 
406
                       
-
 
407
            if(AdWertAccHoch > 1)
-
 
408
             {
-
 
409
              if(NeutralAccZ < 750)
-
 
410
               {
-
 
411
                NeutralAccZ += 0.02;
-
 
412
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
-
 
413
               }
-
 
414
             }  
-
 
415
             else if(AdWertAccHoch < -1)
-
 
416
             {
-
 
417
              if(NeutralAccZ > 550)
-
 
418
                {
-
 
419
                 NeutralAccZ -= 0.02;
-
 
420
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
-
 
421
                }
-
 
422
             }
-
 
423
                        */ // MartinR: deaktiviert Ende 
-
 
424
                         
-
 
425
            // messanzahl_AccHoch = 1; MartinR: deaktiviert, da nicht verwendet
-
 
426
               
-
 
427
                        Aktuell_az = (Aktuell_az + ADC) / 2;
-
 
428
                        AdWertAccHoch =  Aktuell_az - NeutralAccZ; // MartinR: modifiziert für Tests
-
 
429
                        //Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren  // MartinR: zu Höhenregler in fc.c verschoben
-
 
430
            //Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen // MartinR: zu Höhenregler in fc.c verschoben
-
 
431
                       
-
 
432
                        AdReady = 1;
-
 
433
                        state = 0;
-
 
434
                        kanal = AD_NICK;
-
 
435
            break;
-
 
436
                       
-
 
437
        default:  
-
 
438
            kanal = 0; state = 0; kanal = AD_NICK;
-
 
439
            break;
-
 
440
        }
-
 
441
       
-
 
442
        // MartinR : geändert Ende
-
 
443
       
-
 
444
       
240
    ADMUX = kanal;
445
    ADMUX = kanal;
241
    if(state != 0) ANALOG_ON;
446
        // if(state != 0) ANALOG_ON; // MartinR : so war es:
-
 
447
        // Bei jedem Reglerzyklus wurde nur der Ablauf der Reihenfolge der AD-Wandlungen angestoßen. 
-
 
448
        // Solange die Reihe nicht durchgelaufen ist, wurde nach jeder AD-Wandlung die nächste AD-Wandlung über ANALOG_ON gestartet.
-
 
449
       
-
 
450
        // if(state != 0) ANALOG_START; // MartinR verschoben zu timer0
-
 
451
               
242
}
452
}
243
 
453
 
244
 
454
 
245
 
455
 
246
/*
456
/*
247
//#######################################################################################
457
//#######################################################################################
248
//
458
//
249
SIGNAL(SIG_ADC)
459
SIGNAL(SIG_ADC)
250
//#######################################################################################
460
//#######################################################################################
251
{
461
{
252
    static unsigned char kanal=0,state = 0;
462
    static unsigned char kanal=0,state = 0;
253
    static signed int gier1, roll1, nick1;
463
    static signed int gier1, roll1, nick1;
254
        static signed long nick_filter, roll_filter;   
464
        static signed long nick_filter, roll_filter;   
255
        static signed int accy, accx;
465
        static signed int accy, accx;
256
    switch(state++)
466
    switch(state++)
257
        {
467
        {
258
        case 0:
468
        case 0:
259
            nick1 = ADC;
469
            nick1 = ADC;
260
            kanal = AD_ROLL;
470
            kanal = AD_ROLL;
261
            break;
471
            break;
262
        case 1:
472
        case 1:
263
            roll1 = ADC;
473
            roll1 = ADC;
264
                    kanal = AD_GIER;
474
                    kanal = AD_GIER;
265
            break;
475
            break;
266
        case 2:
476
        case 2:
267
            gier1 = ADC;
477
            gier1 = ADC;
268
            kanal = AD_ACC_Y;
478
            kanal = AD_ACC_Y;
269
            break;
479
            break;
270
        case 3:
480
        case 3:
271
            Aktuell_ay = NeutralAccY - ADC;
481
            Aktuell_ay = NeutralAccY - ADC;
272
            accy = Aktuell_ay;
482
            accy = Aktuell_ay;
273
            kanal = AD_NICK;
483
            kanal = AD_NICK;
274
            break;
484
            break;
275
        case 4:
485
        case 4:
276
            nick1 += ADC;
486
            nick1 += ADC;
277
            kanal = AD_ROLL;
487
            kanal = AD_ROLL;
278
            break;
488
            break;
279
        case 5:
489
        case 5:
280
            roll1 += ADC;
490
            roll1 += ADC;
281
                    kanal = AD_ACC_Z;
491
                    kanal = AD_ACC_Z;
282
            break;
492
            break;
283
       case 6:
493
       case 6:
284
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
494
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
285
            if(AdWertAccHoch > 1)
495
            if(AdWertAccHoch > 1)
286
             {
496
             {
287
              if(NeutralAccZ < 750)
497
              if(NeutralAccZ < 750)
288
               {
498
               {
289
                NeutralAccZ += 0.02;
499
                NeutralAccZ += 0.02;
290
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
500
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
291
               }
501
               }
292
             }  
502
             }  
293
             else if(AdWertAccHoch < -1)
503
             else if(AdWertAccHoch < -1)
294
             {
504
             {
295
              if(NeutralAccZ > 550)
505
              if(NeutralAccZ > 550)
296
                {
506
                {
297
                 NeutralAccZ-= 0.02;
507
                 NeutralAccZ-= 0.02;
298
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
508
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
299
                }
509
                }
300
             }
510
             }
301
            messanzahl_AccHoch = 1;
511
            messanzahl_AccHoch = 1;
302
            Aktuell_az = ADC;
512
            Aktuell_az = ADC;
303
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
513
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
304
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
514
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
305
            kanal = AD_NICK;
515
            kanal = AD_NICK;
306
            break;
516
            break;
307
        case 7:
517
        case 7:
308
            nick1 += ADC;
518
            nick1 += ADC;
309
            kanal = AD_ROLL;
519
            kanal = AD_ROLL;
310
            break;
520
            break;
311
        case 8:
521
        case 8:
312
            roll1 += ADC;
522
            roll1 += ADC;
313
                    kanal = AD_ACC_X;
523
                    kanal = AD_ACC_X;
314
            break;
524
            break;
315
        case 9:
525
        case 9:
316
            Aktuell_ax = ADC - NeutralAccX;
526
            Aktuell_ax = ADC - NeutralAccX;
317
            accx =  Aktuell_ax;
527
            accx =  Aktuell_ax;
318
                    kanal = AD_GIER;
528
                    kanal = AD_GIER;
319
            break;
529
            break;
320
        case 10:
530
        case 10:
321
            gier1 += ADC;
531
            gier1 += ADC;
322
            kanal = AD_NICK;
532
            kanal = AD_NICK;
323
            break;
533
            break;
324
        case 11:
534
        case 11:
325
            nick1 += ADC;
535
            nick1 += ADC;
326
            kanal = AD_ROLL;
536
            kanal = AD_ROLL;
327
            break;
537
            break;
328
        case 12:
538
        case 12:
329
            roll1 += ADC;
539
            roll1 += ADC;
330
            kanal = AD_UBAT;
540
            kanal = AD_UBAT;
331
            break;
541
            break;
332
        case 13:
542
        case 13:
333
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
543
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
334
            kanal = AD_ACC_Y;
544
            kanal = AD_ACC_Y;
335
            break;
545
            break;
336
        case 14:
546
        case 14:
337
            Aktuell_ay = NeutralAccY - ADC;
547
            Aktuell_ay = NeutralAccY - ADC;
338
            accy += Aktuell_ay;
548
            accy += Aktuell_ay;
339
            kanal = AD_NICK;
549
            kanal = AD_NICK;
340
            break;
550
            break;
341
        case 15:
551
        case 15:
342
            nick1 += ADC;
552
            nick1 += ADC;
343
            kanal = AD_ROLL;
553
            kanal = AD_ROLL;
344
            break;
554
            break;
345
        case 16:
555
        case 16:
346
            roll1 += ADC;
556
            roll1 += ADC;
347
            kanal = AD_ACC_X;
557
            kanal = AD_ACC_X;
348
            break;
558
            break;
349
        case 17:
559
        case 17:
350
            Aktuell_ax = ADC - NeutralAccX;
560
            Aktuell_ax = ADC - NeutralAccX;
351
            accx += Aktuell_ax;
561
            accx += Aktuell_ax;
352
                    kanal = AD_NICK;
562
                    kanal = AD_NICK;
353
            break;
563
            break;
354
        case 18:
564
        case 18:
355
            nick1 += ADC;
565
            nick1 += ADC;
356
            kanal = AD_ROLL;
566
            kanal = AD_ROLL;
357
            break;
567
            break;
358
        case 19:
568
        case 19:
359
            roll1 += ADC;
569
            roll1 += ADC;
360
                    kanal = AD_GIER;
570
                    kanal = AD_GIER;
361
            break;
571
            break;
362
        case 20:
572
        case 20:
363
            gier1 += ADC;
573
            gier1 += ADC;
364
            kanal = AD_ACC_Y;
574
            kanal = AD_ACC_Y;
365
            break;
575
            break;
366
        case 21:
576
        case 21:
367
            Aktuell_ay = NeutralAccY - ADC;
577
            Aktuell_ay = NeutralAccY - ADC;
368
            accy += Aktuell_ay;
578
            accy += Aktuell_ay;
369
            kanal = AD_NICK;
579
            kanal = AD_NICK;
370
            break;
580
            break;
371
        case 22:
581
        case 22:
372
            nick1 += ADC;
582
            nick1 += ADC;
373
            kanal = AD_ROLL;
583
            kanal = AD_ROLL;
374
            break;
584
            break;
375
        case 23:
585
        case 23:
376
            roll1 += ADC;
586
            roll1 += ADC;
377
                kanal = AD_DRUCK;
587
                kanal = AD_DRUCK;
378
            break;
588
            break;
379
        case 24:
589
        case 24:
380
            tmpLuftdruck += ADC;
590
            tmpLuftdruck += ADC;
381
            if(++messanzahl_Druck >= 5)
591
            if(++messanzahl_Druck >= 5)
382
                {
592
                {
383
                MessLuftdruck = ADC;
593
                MessLuftdruck = ADC;
384
                messanzahl_Druck = 0;
594
                messanzahl_Druck = 0;
385
                                HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8;  // D-Anteil = neuerWert - AlterWert
595
                                HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert)) / 8;  // D-Anteil = neuerWert - AlterWert
386
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
596
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
387
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
597
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
388
                tmpLuftdruck = 0;
598
                tmpLuftdruck = 0;
389
                }
599
                }
390
            kanal = AD_NICK;
600
            kanal = AD_NICK;
391
            break;
601
            break;
392
        case 25:
602
        case 25:
393
            nick1 += ADC;
603
            nick1 += ADC;
394
            kanal = AD_ROLL;
604
            kanal = AD_ROLL;
395
            break;
605
            break;
396
        case 26:
606
        case 26:
397
            roll1 += ADC;
607
            roll1 += ADC;
398
                    kanal = AD_ACC_X;
608
                    kanal = AD_ACC_X;
399
            break;
609
            break;
400
        case 27:
610
        case 27:
401
            Aktuell_ax = ADC - NeutralAccX;
611
            Aktuell_ax = ADC - NeutralAccX;
402
            accx +=  Aktuell_ax;
612
            accx +=  Aktuell_ax;
403
                    kanal = AD_GIER;
613
                    kanal = AD_GIER;
404
            break;
614
            break;
405
        case 28:
615
        case 28:
406
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 2) / 4;
616
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1 + 2) / 4;
407
            else
617
            else
408
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
618
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1 + 1) / 2;
409
                        else                                       AdWertGier = (ADC + gier1 + 1) / 2;
619
                        else                                       AdWertGier = (ADC + gier1 + 1) / 2;
410
            kanal = AD_NICK;
620
            kanal = AD_NICK;
411
            break;
621
            break;
412
        case 29:
622
        case 29:
413
            nick1 += ADC;
623
            nick1 += ADC;
414
            kanal = AD_ROLL;
624
            kanal = AD_ROLL;
415
            break;
625
            break;
416
        case 30:
626
        case 30:
417
            roll1 += ADC;
627
            roll1 += ADC;
418
            kanal = AD_ACC_Y;
628
            kanal = AD_ACC_Y;
419
            break;
629
            break;
420
        case 31:
630
        case 31:
421
            Aktuell_ay = NeutralAccY - ADC;
631
            Aktuell_ay = NeutralAccY - ADC;
422
            AdWertAccRoll = (Aktuell_ay + accy);
632
            AdWertAccRoll = (Aktuell_ay + accy);
423
            kanal = AD_NICK;
633
            kanal = AD_NICK;
424
            break;
634
            break;
425
        case 32:
635
        case 32:
426
            AdWertNick = (ADC + nick1 + 3) / 5;
636
            AdWertNick = (ADC + nick1 + 3) / 5;
427
            nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
637
            nick_filter = (long) (1 * (long) nick_filter + 4 * (long)(ADC + nick1) + 1) / 2;
428
            if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
638
            if(PlatinenVersion == 10) { AdWertNick /= 2;nick_filter /=2;}
429
            HiResNick = nick_filter - 20 * AdNeutralNick;
639
            HiResNick = nick_filter - 20 * AdNeutralNick;
430
            AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
640
            AdWertNickFilter = (long)(3L * (long)AdWertNickFilter + HiResNick + 2) / 4;
431
            DebugOut.Analog[21] = AdWertNickFilter / 4;  
641
            DebugOut.Analog[21] = AdWertNickFilter / 4;  
432
            kanal = AD_ROLL;
642
            kanal = AD_ROLL;
433
            break;
643
            break;
434
        case 33:
644
        case 33:
435
            AdWertRoll = (ADC + roll1 + 3) / 5;
645
            AdWertRoll = (ADC + roll1 + 3) / 5;
436
            roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
646
            roll_filter = (long)(1 * (long)roll_filter + 4 * (long)(ADC + roll1) + 1) / 2;
437
            if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
647
            if(PlatinenVersion == 10) { AdWertRoll /= 2;roll_filter /=2;}
438
            HiResRoll = roll_filter - 20 * AdNeutralRoll;
648
            HiResRoll = roll_filter - 20 * AdNeutralRoll;
439
            AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
649
            AdWertRollFilter = (long)(3L * (long)AdWertRollFilter + HiResRoll + 2) / 4;
440
            DebugOut.Analog[22] = AdWertRollFilter / 4;
650
            DebugOut.Analog[22] = AdWertRollFilter / 4;
441
            kanal = AD_ACC_X;
651
            kanal = AD_ACC_X;
442
            break;
652
            break;
443
        case 34:
653
        case 34:
444
            Aktuell_ax = ADC - NeutralAccX;
654
            Aktuell_ax = ADC - NeutralAccX;
445
            AdWertAccNick =  (Aktuell_ax + accx);
655
            AdWertAccNick =  (Aktuell_ax + accx);
446
                    kanal = AD_NICK;
656
                    kanal = AD_NICK;
447
            state = 0;
657
            state = 0;
448
                        AdReady = 1;
658
                        AdReady = 1;
449
            ZaehlMessungen++;
659
            ZaehlMessungen++;
450
            break;
660
            break;
451
        default:
661
        default:
452
            kanal = 0;
662
            kanal = 0;
453
            state = 0;
663
            state = 0;
454
            break;
664
            break;
455
        }
665
        }
456
    ADMUX = kanal;
666
    ADMUX = kanal;
457
    if(state != 0) ANALOG_ON;
667
    if(state != 0) ANALOG_ON;
458
}
668
}
459
*/
669
*/
460
 
670