Subversion Repositories FlightCtrl

Rev

Rev 1111 | Rev 1166 | Go to most recent revision | Details | Compare with Previous | Last modification | View Log | RSS feed

Rev Author Line No. Line
1 ingob 1
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
2
// + Copyright (c) 04.2007 Holger Buss
3
// + only for non-profit use
4
// + www.MikroKopter.com
5
// + see the File "License.txt" for further Informations
6
// ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++
7
 
8
#include "main.h"
9
volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
380 hbuss 10
volatile int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
11
volatile int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
1 ingob 12
volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0;
13
volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0;
14
volatile long Luftdruck = 32000;
15
volatile int  StartLuftdruck;
16
volatile unsigned int  MessLuftdruck = 1023;
17
unsigned char DruckOffsetSetting;
1036 hbuss 18
signed char ExpandBaro = 0;
1 ingob 19
volatile int HoeheD = 0;
20
volatile char messanzahl_Druck;
21
volatile int  tmpLuftdruck;
22
volatile unsigned int ZaehlMessungen = 0;
918 hbuss 23
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
24
unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
1155 hbuss 25
unsigned char AdReady = 1;
1 ingob 26
//#######################################################################################
27
//
28
void ADC_Init(void)
29
//#######################################################################################
30
{
31
    ADMUX = 0;//Referenz ist extern
1155 hbuss 32
    ANALOG_ON;
1 ingob 33
}
34
 
35
void SucheLuftruckOffset(void)
36
{
37
 unsigned int off;
173 holgerb 38
 off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
39
 if(off > 20) off -= 10;
40
 OCR0A = off;
1036 hbuss 41
 ExpandBaro = 0;
380 hbuss 42
 Delay_ms_Mess(100);
173 holgerb 43
 if(MessLuftdruck < 850) off = 0;
44
 for(; off < 250;off++)
1 ingob 45
  {
46
  OCR0A = off;
380 hbuss 47
  Delay_ms_Mess(50);
1 ingob 48
  printf(".");  
1036 hbuss 49
  if(MessLuftdruck < 850) break;
1 ingob 50
  }
173 holgerb 51
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
52
 DruckOffsetSetting = off;
380 hbuss 53
 Delay_ms_Mess(300);
1 ingob 54
}
55
 
918 hbuss 56
void SucheGyroOffset(void)
57
{
58
 unsigned char i, ready = 0;
59
 GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
60
 for(i=140; i != 0; i--)
61
  {
921 hbuss 62
   if(ready == 3 && i > 10) i = 9;
918 hbuss 63
   ready = 0;
64
   if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; else ready++;
65
   if(AdWertRoll < 1020) AnalogOffsetRoll--; else if(AdWertRoll > 1030) AnalogOffsetRoll++; else ready++;
66
   if(AdWertGier < 1020) AnalogOffsetGier--; else if(AdWertGier > 1030) AnalogOffsetGier++; else ready++;
67
   twi_state = 8;  
68
   i2c_start();
69
   if(AnalogOffsetNick < 10)  { GyroDefektN = 1; AnalogOffsetNick = 10;}; if(AnalogOffsetNick > 245) { GyroDefektN = 1; AnalogOffsetNick = 245;};
70
   if(AnalogOffsetRoll < 10)  { GyroDefektR = 1; AnalogOffsetRoll = 10;}; if(AnalogOffsetRoll > 245) { GyroDefektR = 1; AnalogOffsetRoll = 245;};
71
   if(AnalogOffsetGier < 10)  { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
72
   while(twi_state);
73
   messanzahl_Druck = 0;
74
   ANALOG_ON;
75
   while(messanzahl_Druck == 0);
76
   if(i<10) Delay_ms_Mess(10);  
77
  }
921 hbuss 78
   Delay_ms_Mess(70);  
918 hbuss 79
}
1 ingob 80
 
81
//#######################################################################################
82
//
83
SIGNAL(SIG_ADC)
84
//#######################################################################################
85
{
86
    static unsigned char kanal=0,state = 0;
1155 hbuss 87
    static unsigned int gier1, roll1, nick1, accy, accx;
1 ingob 88
    ANALOG_OFF;
89
    switch(state++)
90
        {
91
        case 0:
401 hbuss 92
            gier1 = ADC;
1155 hbuss 93
            kanal = AD_ROLL;
1 ingob 94
            ZaehlMessungen++;
95
            break;
96
        case 1:
401 hbuss 97
            roll1 = ADC;
1155 hbuss 98
            kanal = AD_NICK;
1 ingob 99
            break;
100
        case 2:
401 hbuss 101
            nick1 = ADC;
1155 hbuss 102
            kanal = AD_ACC_Y;
1 ingob 103
            break;
104
        case 3:
1155 hbuss 105
            Aktuell_ay = NeutralAccY - ADC;
106
            accy = Aktuell_ay;
107
            kanal = AD_ACC_X;
1 ingob 108
            break;
109
        case 4:
1155 hbuss 110
            Aktuell_ax = ADC - NeutralAccX;
111
            accx =  Aktuell_ax;
112
                    kanal = AD_GIER;
1 ingob 113
            break;
114
        case 5:
1155 hbuss 115
            gier1 += ADC;
116
            kanal = AD_ROLL;
1 ingob 117
            break;
118
        case 6:
1155 hbuss 119
            roll1 += ADC;
120
            kanal = AD_NICK;
401 hbuss 121
            break;
122
        case 7:
1155 hbuss 123
            nick1 += ADC;
124
            kanal = AD_UBAT;
401 hbuss 125
            break;
126
        case 8:
1155 hbuss 127
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
128
            kanal = AD_ACC_Z;
401 hbuss 129
            break;
130
       case 9:
380 hbuss 131
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
132
            if(AdWertAccHoch > 1)
1 ingob 133
             {
871 hbuss 134
              if(NeutralAccZ < 750)
135
               {
136
                NeutralAccZ += 0.02;
137
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
138
               }
1 ingob 139
             }  
380 hbuss 140
             else if(AdWertAccHoch < -1)
1 ingob 141
             {
871 hbuss 142
              if(NeutralAccZ > 550)
143
                {
144
                 NeutralAccZ-= 0.02;
145
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
146
                }
1 ingob 147
             }
148
            messanzahl_AccHoch = 1;
149
            Aktuell_az = ADC;
380 hbuss 150
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
1 ingob 151
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
1155 hbuss 152
            kanal = AD_GIER;
1 ingob 153
            break;
401 hbuss 154
        case 10:
1155 hbuss 155
            gier1 += ADC;
156
            kanal = AD_ROLL;
157
            break;
158
        case 11:
159
            roll1 += ADC;
160
            kanal = AD_NICK;
161
            break;
162
        case 12:
163
            nick1 += ADC;
164
                kanal = AD_DRUCK;
165
            break;
166
        case 13:
1 ingob 167
            tmpLuftdruck += ADC;
168
            if(++messanzahl_Druck >= 5)
169
                {
170
                MessLuftdruck = ADC;
171
                messanzahl_Druck = 0;
1036 hbuss 172
                                HoeheD = (7 * HoeheD + (int) Parameter_Luftdruck_D * (int)(255 * ExpandBaro + StartLuftdruck - tmpLuftdruck - HoehenWert))/8;  // D-Anteil = neuerWert - AlterWert
1 ingob 173
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
1036 hbuss 174
                HoehenWert = 255 * ExpandBaro + StartLuftdruck - Luftdruck;
1 ingob 175
                tmpLuftdruck = 0;
176
                }
1155 hbuss 177
            kanal = AD_ACC_Y;
178
            break;
179
        case 14:
180
            Aktuell_ay = NeutralAccY - ADC;
181
            AdWertAccRoll = (Aktuell_ay + accy);
182
            kanal = AD_ACC_X;
183
            break;
184
        case 15:
185
            Aktuell_ax = ADC - NeutralAccX;
186
            AdWertAccNick =  (Aktuell_ax + accx);
187
                    kanal = AD_GIER;
188
            break;
189
        case 16:
190
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1) / 4;
191
            else
192
            if(PlatinenVersion == 20)  AdWertGier = 2047 - (ADC + gier1)/2;
193
                        else                                       AdWertGier = (ADC + gier1) / 2;
194
            kanal = AD_ROLL;
195
            break;
196
        case 17:
197
            if(PlatinenVersion == 10)  AdWertRoll = (ADC + roll1) / 4;
198
                        else                                       AdWertRoll = (ADC + roll1) / 2;
199
            kanal = AD_NICK;
200
            break;
201
        case 18:
202
            if(PlatinenVersion == 10)  AdWertNick = (ADC + nick1) / 4;
203
                        else                                       AdWertNick = (ADC + nick1) / 2;
204
            kanal = AD_GIER;
1 ingob 205
            state = 0;
1155 hbuss 206
                        AdReady = 1;
1 ingob 207
            break;
208
        default:
209
            kanal = 0;
210
            state = 0;
211
            break;
212
        }
213
    ADMUX = kanal;
380 hbuss 214
    if(state != 0) ANALOG_ON;
1 ingob 215
}