Subversion Repositories FlightCtrl

Rev

Rev 1021 | Rev 1111 | Go to most recent revision | Only display areas with differences | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed

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