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;
37
 for(off=0; off < 250;off++)
38
  {
39
  OCR0A = off;
40
  Delay_ms(50);
41
  printf(".");  
42
  if(MessLuftdruck < 900) break;
43
  }
44
   DruckOffsetSetting = off;
45
   Delay_ms(200);
46
}
47
 
48
 
49
//#######################################################################################
50
//
51
SIGNAL(SIG_ADC)
52
//#######################################################################################
53
{
54
    static unsigned char kanal=0,state = 0;
55
    signed int wert;   
56
    ANALOG_OFF;
57
    switch(state++)
58
        {
59
        case 0:
60
            wert = (signed int) AdNeutralGier - ADC;
61
            AccumulateGier += wert;     //
62
            MessanzahlGier++;
63
            Mess_Integral_Gier += wert;// / 16;
64
            Mess_Integral_Gier2 += wert;
124 salvo 65
                        GyroKomp_Int  += wert;
66
                        GyroKomp_Int2 += wert;                         
1 ingob 67
            kanal = 1;
68
            ZaehlMessungen++;
69
            break;
70
        case 1:
71
            wert = (signed int) ADC - AdNeutralRoll;
72
            Mess_IntegralRoll += wert;
73
            Mess_IntegralRoll2 += wert;
74
            if(ADC < 10)   wert = -700;
75
            if(ADC > 1000) wert = +700;
76
            AccumulateRoll += wert;
77
            MessanzahlRoll++;
78
            kanal = 2;
79
            break;
80
        case 2:
81
            wert = (signed int) ADC - AdNeutralNick;
82
            Mess_IntegralNick += wert;
83
            Mess_IntegralNick2 += wert;
84
            if(ADC < 10)   wert = -700;
85
            if(ADC > 1000) wert = +700;
86
            AccumulateNick += wert;
87
            MessanzahlNick++;
88
            kanal = 4;
89
            break;
90
        case 3:
91
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
92
            kanal = 6;
93
            break;
94
        case 4:
95
            Aktuell_ay = NeutralAccY - ADC;
96
            accumulate_AccRoll += Aktuell_ay;
97
            messanzahl_AccRoll++;
98
            kanal = 7;
99
            break;
100
        case 5:
101
            Aktuell_ay = ADC - NeutralAccX;
102
            accumulate_AccNick +=  Aktuell_ay;
103
            messanzahl_AccNick++;
104
                    kanal = 5;
105
                    state = 6;
106
            break;
107
        case 6:
108
            accumulate_AccHoch =  (signed int) ADC - NeutralAccZ;
109
            accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
110
            if(accumulate_AccHoch > 1)
111
             {
117 salvo 112
            if(NeutralAccZ < 800) NeutralAccZ+= 0.02;
1 ingob 113
             }  
114
             else if(accumulate_AccHoch < -1)
115
             {
116
              if(NeutralAccZ > 600) NeutralAccZ-= 0.02;
117
             }
118
            messanzahl_AccHoch = 1;
119
            Aktuell_az = ADC;
120
            Mess_Integral_Hoch += accumulate_AccHoch;      // Integrieren
121
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämfen
122
//            Mess_Integral_Hoch -= Mess_Integral_Hoch / 512; // dämfen
123
 /*           if(EE_Parameter.GlobalConfig & CFG_HOEHENREGELUNG)
124
             {
125
                      kanal = 3;
126
                      state = 7;
127
             }
128
            else  
129
             {
130
                      kanal = 0;
131
                      state = 0;
132
             }*/
133
                      kanal = 3;
134
                      state = 7;
135
            break;
136
        case 7:
137
            tmpLuftdruck += ADC;
138
            if(++messanzahl_Druck >= 5)
139
                {
140
                MessLuftdruck = ADC;
141
                messanzahl_Druck = 0;
142
                                HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert);  // D-Anteil = neuerWert - AlterWert
143
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
144
                HoehenWert = StartLuftdruck - Luftdruck;
145
                tmpLuftdruck = 0;
146
                }
147
            kanal = 0;
148
            state = 0;
149
            break;
150
        default:
151
            kanal = 0;
152
            state = 0;
153
            break;
154
        }
155
    ADMUX = kanal;
156
    ANALOG_ON;
157
}