Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
1068 thjac 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
 
10
volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
11
volatile int  AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0;
12
volatile int  AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0;
13
volatile char MessanzahlNick = 0, MessanzahlRoll = 0, MessanzahlGier = 0;
14
volatile char messanzahl_AccNick = 0, messanzahl_AccRoll = 0, messanzahl_AccHoch = 0;
15
volatile long Luftdruck = 32000;
16
volatile int  StartLuftdruck;
17
volatile unsigned int  MessLuftdruck = 1023;
18
unsigned char DruckOffsetSetting;
19
volatile int HoeheD = 0;
20
volatile char messanzahl_Druck;
21
volatile int  tmpLuftdruck;
22
volatile unsigned int ZaehlMessungen = 0;
23
unsigned char AnalogOffsetNick = 115,AnalogOffsetRoll = 115,AnalogOffsetGier = 115;
24
unsigned char GyroDefektN = 0,GyroDefektR = 0,GyroDefektG = 0;
25
 
26
int airPressure = 0;
27
 
28
 
29
//#######################################################################################
30
//
31
void ADC_Init(void)
32
//#######################################################################################
33
{
34
    ADMUX = 0;//Referenz ist extern
35
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
36
    //Free Running Mode, Division Factor 128, Interrupt on
37
}
38
 
39
void SucheLuftruckOffset(void)
40
{
41
 unsigned int off;
42
 off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
43
 if(off > 20) off -= 10;
44
 OCR0A = off;
45
 Delay_ms_Mess(100);
46
 if(MessLuftdruck < 850) off = 0;
47
 for(; off < 250;off++)
48
  {
49
  OCR0A = off;
50
  Delay_ms_Mess(50);
51
  printf(".");  
52
  if(MessLuftdruck < 900) break;
53
  }
54
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
55
 DruckOffsetSetting = off;
56
 Delay_ms_Mess(300);
57
}
58
 
59
void SucheGyroOffset(void)
60
{
61
 unsigned char i, ready = 0;
62
 GyroDefektN = 0; GyroDefektR = 0; GyroDefektG = 0;
63
 for(i=140; i != 0; i--)
64
  {
65
   if(ready == 3 && i > 10) i = 9;
66
   ready = 0;
67
   if(AdWertNick < 1020) AnalogOffsetNick--; else if(AdWertNick > 1030) AnalogOffsetNick++; 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++;
70
   twi_state = 8;  
71
   i2c_start();
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;};
74
   if(AnalogOffsetGier < 10)  { GyroDefektG = 1; AnalogOffsetGier = 10;}; if(AnalogOffsetGier > 245) { GyroDefektG = 1; AnalogOffsetGier = 245;};
75
   while(twi_state);
76
   messanzahl_Druck = 0;
77
   ANALOG_ON;
78
   while(messanzahl_Druck == 0);
79
   if(i<10) Delay_ms_Mess(10);  
80
  }
81
   Delay_ms_Mess(70);  
82
}
83
 
84
 
85
//#######################################################################################
86
//
87
SIGNAL(SIG_ADC)
88
//#######################################################################################
89
{
90
    static unsigned char kanal=0,state = 0;
91
    static unsigned int gier1, roll1, nick1;
92
    ANALOG_OFF;
93
    switch(state++)
94
        {
95
        case 0:
96
            gier1 = ADC;
97
            kanal = 1;
98
            ZaehlMessungen++;
99
            break;
100
        case 1:
101
            roll1 = ADC;
102
            kanal = 2;
103
            break;
104
        case 2:
105
            nick1 = ADC;
106
            kanal = 4;
107
            break;
108
        case 3:
109
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
110
            kanal = 6;
111
            break;
112
        case 4:
113
            Aktuell_ay = NeutralAccY - ADC;
114
            AdWertAccRoll = Aktuell_ay;
115
            kanal = 7;
116
            break;
117
        case 5:
118
            Aktuell_ax = ADC - NeutralAccX;
119
            AdWertAccNick =  Aktuell_ax;
120
                    kanal = 0;
121
            break;
122
        case 6:
123
            if(PlatinenVersion == 10)  AdWertGier = (ADC + gier1) / 2;
124
                        else                                       AdWertGier = ADC + gier1;
125
            kanal = 1;
126
            break;
127
        case 7:
128
            if(PlatinenVersion == 10)  AdWertRoll = (ADC + roll1) / 2;
129
                        else                                       AdWertRoll = ADC + roll1;
130
            kanal = 2;
131
            break;
132
        case 8:
133
            if(PlatinenVersion == 10)  AdWertNick = (ADC + nick1) / 2;
134
                        else                                       AdWertNick = ADC + nick1;
135
//AdWertNick = 0;
136
//AdWertNick += Poti2;            
137
            kanal = 5;
138
            break;
139
       case 9:
140
            AdWertAccHoch =  (signed int) ADC - NeutralAccZ;
141
//            AdWertAccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
142
            if(AdWertAccHoch > 1)
143
             {
144
              if(NeutralAccZ < 750)
145
               {
146
                NeutralAccZ += 0.02;
147
                if(modell_fliegt < 500) NeutralAccZ += 0.1;
148
               }
149
             }  
150
             else if(AdWertAccHoch < -1)
151
             {
152
              if(NeutralAccZ > 550)
153
                {
154
                 NeutralAccZ-= 0.02;
155
                 if(modell_fliegt < 500) NeutralAccZ -= 0.1;              
156
                }
157
             }
158
            messanzahl_AccHoch = 1;
159
            Aktuell_az = ADC;
160
            Mess_Integral_Hoch += AdWertAccHoch;      // Integrieren
161
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
162
                kanal = 3;
163
            break;
164
        case 10:
165
            tmpLuftdruck += ADC;
166
            if(++messanzahl_Druck >= 5) {
167
                MessLuftdruck = ADC;
168
                messanzahl_Druck = 0;
169
 
170
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
171
                airPressure = StartLuftdruck - Luftdruck;
172
                tmpLuftdruck = 0;
173
            }
174
 
175
            kanal = 0;
176
            state = 0;
177
            break;
178
        default:
179
            kanal = 0;
180
            state = 0;
181
            break;
182
        }
183
    ADMUX = kanal;
184
    if(state != 0) ANALOG_ON;
185
}