Subversion Repositories FlightCtrl

Rev

Details | 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
 
10
volatile int  Aktuell_Nick,Aktuell_Roll,Aktuell_Gier,Aktuell_ax, Aktuell_ay,Aktuell_az, UBat = 100;
11
volatile int  AccumulateNick = 0, AccumulateRoll = 0, AccumulateGier = 0;
12
volatile int  accumulate_AccRoll = 0,accumulate_AccNick = 0,accumulate_AccHoch = 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
 
24
//#######################################################################################
25
//
26
void ADC_Init(void)
27
//#######################################################################################
28
{
29
    ADMUX = 0;//Referenz ist extern
30
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
31
    //Free Running Mode, Division Factor 128, Interrupt on
32
}
33
 
34
void SucheLuftruckOffset(void)
35
{
36
 unsigned int off;
173 holgerb 37
 off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]);
38
 if(off > 20) off -= 10;
39
 OCR0A = off;
40
 Delay_ms(100);
41
 if(MessLuftdruck < 850) off = 0;
42
 for(; off < 250;off++)
1 ingob 43
  {
44
  OCR0A = off;
45
  Delay_ms(50);
46
  printf(".");  
47
  if(MessLuftdruck < 900) break;
48
  }
173 holgerb 49
 eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off);
50
 DruckOffsetSetting = off;
51
 Delay_ms(300);
1 ingob 52
}
53
 
54
 
55
//#######################################################################################
56
//
57
SIGNAL(SIG_ADC)
58
//#######################################################################################
59
{
60
    static unsigned char kanal=0,state = 0;
61
    signed int wert;   
62
    ANALOG_OFF;
63
    switch(state++)
64
        {
65
        case 0:
66
            wert = (signed int) AdNeutralGier - ADC;
173 holgerb 67
            if(PlatinenVersion != 10) wert *= 2;
1 ingob 68
            AccumulateGier += wert;     //
69
            MessanzahlGier++;
70
            Mess_Integral_Gier += wert;// / 16;
71
            Mess_Integral_Gier2 += wert;
330 salvo 72
                        GyroKomp_Int  += wert;
1 ingob 73
            kanal = 1;
74
            ZaehlMessungen++;
75
            break;
76
        case 1:
77
            wert = (signed int) ADC - AdNeutralRoll;
173 holgerb 78
            if(PlatinenVersion != 10) wert *= 2;
1 ingob 79
            Mess_IntegralRoll += wert;
80
            Mess_IntegralRoll2 += wert;
173 holgerb 81
            if(Mess_IntegralRoll > 310000L) Mess_IntegralRoll = -290000L;
82
            if(Mess_IntegralRoll <-310000L) Mess_IntegralRoll = 290000L;
83
            if(ADC < 15)   wert = -1000;
84
            if(ADC <  7)   wert = -2000;
85
            if(ADC > 1010) wert = +1000;
86
            if(ADC > 1017) wert = +2000;
1 ingob 87
            AccumulateRoll += wert;
88
            MessanzahlRoll++;
89
            kanal = 2;
90
            break;
91
        case 2:
92
            wert = (signed int) ADC - AdNeutralNick;
173 holgerb 93
            if(PlatinenVersion != 10) wert *= 2;
1 ingob 94
            Mess_IntegralNick += wert;
95
            Mess_IntegralNick2 += wert;
173 holgerb 96
            if(Mess_IntegralNick > 310000L) Mess_IntegralNick = -290000L;
97
            if(Mess_IntegralNick <-310000L) Mess_IntegralNick = 290000L;
98
            if(ADC < 15)   wert = -1000;
99
            if(ADC <  7)   wert = -2000;
100
            if(ADC > 1010) wert = +1000;
101
            if(ADC > 1017) wert = +2000;
1 ingob 102
            AccumulateNick += wert;
103
            MessanzahlNick++;
104
            kanal = 4;
105
            break;
106
        case 3:
107
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
108
            kanal = 6;
109
            break;
110
        case 4:
111
            Aktuell_ay = NeutralAccY - ADC;
112
            accumulate_AccRoll += Aktuell_ay;
113
            messanzahl_AccRoll++;
114
            kanal = 7;
115
            break;
116
        case 5:
128 ingob 117
            Aktuell_ax = ADC - NeutralAccX;
118
            accumulate_AccNick +=  Aktuell_ax;
1 ingob 119
            messanzahl_AccNick++;
120
                    kanal = 5;
121
                    state = 6;
122
            break;
123
        case 6:
124
            accumulate_AccHoch =  (signed int) ADC - NeutralAccZ;
125
            accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
126
            if(accumulate_AccHoch > 1)
127
             {
128
              if(NeutralAccZ < 800) NeutralAccZ+= 0.02;
129
             }  
130
             else if(accumulate_AccHoch < -1)
131
             {
132
              if(NeutralAccZ > 600) NeutralAccZ-= 0.02;
133
             }
134
            messanzahl_AccHoch = 1;
135
            Aktuell_az = ADC;
136
            Mess_Integral_Hoch += accumulate_AccHoch;      // Integrieren
137
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
138
//            Mess_Integral_Hoch -= Mess_Integral_Hoch / 512; // dämfen
139
 /*           if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
140
             {
141
                      kanal = 3;
142
                      state = 7;
143
             }
144
            else  
145
             {
146
                      kanal = 0;
147
                      state = 0;
148
             }*/
149
                      kanal = 3;
150
                      state = 7;
151
            break;
152
        case 7:
153
            tmpLuftdruck += ADC;
154
            if(++messanzahl_Druck >= 5)
155
                {
156
                MessLuftdruck = ADC;
157
                messanzahl_Druck = 0;
158
                                HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert);  // D-Anteil = neuerWert - AlterWert
159
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
160
                HoehenWert = StartLuftdruck - Luftdruck;
161
                tmpLuftdruck = 0;
162
                }
163
            kanal = 0;
164
            state = 0;
165
            break;
166
        default:
167
            kanal = 0;
168
            state = 0;
169
            break;
170
        }
171
    ADMUX = kanal;
172
    ANALOG_ON;
173
}