Subversion Repositories FlightCtrl

Rev

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

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