Subversion Repositories FlightCtrl

Rev

Details | Last modification | View Log | RSS feed

Rev Author Line No. Line
561 Nick666 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
//
27
void ADC_Init(void)
28
//#######################################################################################
29
{
30
    ADMUX = 0;//Referenz ist extern
31
    ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE);
32
    //Free Running Mode, Division Factor 128, Interrupt on
33
}
34
 
35
 
36
//#######################################################################################
37
//
38
void SucheLuftruckOffset(void)
39
//#######################################################################################
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(100);
46
 if(MessLuftdruck < 850) off = 0;
47
 for(; off < 250;off++)
48
  {
49
  OCR0A = off;
50
  Delay_ms(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(200);
57
}
58
 
59
 
60
//#######################################################################################
61
//
62
SIGNAL(SIG_ADC)
63
//#######################################################################################
64
{
65
    static unsigned char kanal=0,state = 0;
66
    signed int wert;   
67
    ANALOG_OFF;
68
    switch(state++)
69
        {
70
        case 0:
71
            wert = (signed int) AdNeutralGier - ADC;
72
            AccumulateGier += wert;     //
73
            MessanzahlGier++;
74
            Mess_Integral_Gier += wert;// / 16;
75
            Mess_Integral_Gier2 += wert;
76
            kanal = 1;
77
            ZaehlMessungen++;
78
            break;
79
        case 1:
80
            wert = (signed int) ADC - AdNeutralRoll;
81
            Mess_IntegralRoll += wert;
82
            Mess_IntegralRoll2 += wert;
83
            if(ADC < 10)   wert = -700;
84
            if(ADC > 1000) wert = +700;
85
            AccumulateRoll += wert;
86
            MessanzahlRoll++;
87
            kanal = 2;
88
            break;
89
        case 2:
90
            wert = (signed int) ADC - AdNeutralNick;
91
            Mess_IntegralNick += wert;
92
            Mess_IntegralNick2 += wert;
93
            if(ADC < 10)   wert = -700;
94
            if(ADC > 1000) wert = +700;
95
            AccumulateNick += wert;
96
            MessanzahlNick++;
97
            kanal = 4;
98
            break;
99
        case 3:
100
            UBat = (3 * UBat + ADC / 3) / 4;//(UBat + ((ADC * 39) / 256) + 19)  / 2;
101
            kanal = 6;
102
            break;
103
        case 4:
104
            Aktuell_ay = acc_neutral.Y - ADC;
105
            accumulate_AccRoll += Aktuell_ay;
106
            messanzahl_AccRoll++;
107
            kanal = 7;
108
            break;
109
        case 5:
110
            Aktuell_ax = ADC - acc_neutral.X;
111
            accumulate_AccNick +=  Aktuell_ax;
112
            messanzahl_AccNick++;
113
                    kanal = 5;
114
                    state = 6;
115
            break;
116
        case 6:
117
            accumulate_AccHoch =  (signed int) ADC - acc_neutral.Z;
118
            accumulate_AccHoch += abs(Aktuell_ay) / 4 + abs(Aktuell_ax) / 4;
119
            if(accumulate_AccHoch > 1)
120
             {
121
              if(acc_neutral.Z < 800) acc_neutral.Z+= 0.02;
122
             }  
123
             else if(accumulate_AccHoch < -1)
124
             {
125
              if(acc_neutral.Z > 600) acc_neutral.Z-= 0.02;
126
             }
127
            messanzahl_AccHoch = 1;
128
            Aktuell_az = ADC;
129
            Mess_Integral_Hoch += accumulate_AccHoch;      // Integrieren
130
            Mess_Integral_Hoch -= Mess_Integral_Hoch / 1024; // dämpfen
131
                    kanal = 3;
132
                    state = 7;
133
            break;
134
        case 7:
135
            tmpLuftdruck += ADC;
136
            if(++messanzahl_Druck >= 5)
137
                {
138
                MessLuftdruck = ADC;
139
                messanzahl_Druck = 0;
140
                                HoeheD = (int)(StartLuftdruck - tmpLuftdruck - HoehenWert);  // D-Anteil = neuerWert - AlterWert
141
                Luftdruck = (tmpLuftdruck + 3 * Luftdruck) / 4;
142
                HoehenWert = StartLuftdruck - Luftdruck;
143
                tmpLuftdruck = 0;
144
                }
145
            kanal = 0;
146
            state = 0;
147
            break;
148
        default:
149
            kanal = 0;
150
            state = 0;
151
            break;
152
        }
153
    ADMUX = kanal;
154
    ANALOG_ON;
155
}