Rev 838 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 838 | Rev 966 | ||
---|---|---|---|
Line 11... | Line 11... | ||
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
11 | are not written by Michael Walter are under the license by H. Buss and I. Busker (license_buss.txt) published by www.mikrokopter.de |
12 | unless it is stated otherwise. |
12 | unless it is stated otherwise. |
13 | */ |
13 | */ |
Line 14... | Line 14... | ||
14 | 14 | ||
15 | #include "main.h" |
15 | #include "main.h" |
Line -... | Line 16... | ||
- | 16 | #include "FlightControl.h" |
|
16 | #include "KalmanFc.h" |
17 | |
17 | 18 | ||
18 | int UBat = 100; |
19 | int UBat = 100; /* Initial Battery Guess */ |
19 | int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0; |
20 | int AdWertNick = 0, AdWertRoll = 0, AdWertGier = 0; |
20 | int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0; |
21 | int AdWertAccRoll = 0,AdWertAccNick = 0,AdWertAccHoch = 0; |
Line 21... | Line 22... | ||
21 | int AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0; |
22 | int AdWertNick_Raw = 0, AdWertRoll_Raw = 0, AdWertGier_Raw = 0; |
22 | int AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0; |
23 | int AdWertAccRoll_Raw = 0,AdWertAccNick_Raw = 0,AdWertAccHoch_Raw = 0; |
23 | 24 | ||
24 | int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0; |
25 | int AccumulatedACC_X = 0, AccumulatedACC_Y = 0, AccumulatedACC_Z = 0, AccumulatedAirPressure = 0; |
Line 25... | Line 26... | ||
25 | int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0; |
26 | int AccumulatedACC_X_cnt = 0, AccumulatedACC_Y_cnt = 0, AccumulatedACC_Z_cnt = 0, AccumulatedAirPressure_cnt = 0; |
Line 26... | Line 27... | ||
26 | int AccumulatedRoll_X = 0, AccumulatedRoll_Y = 0, AccumulatedRoll_Z = 0; |
27 | int AccumulatedRoll_X = 0, AccumulatedRoll_Y = 0, AccumulatedRoll_Z = 0; |
27 | int AccumulatedRoll_X_cnt = 0, AccumulatedRoll_Y_cnt = 0, AccumulatedRoll_Z_cnt = 0; |
28 | int AccumulatedRoll_X_cnt = 0, AccumulatedRoll_Y_cnt = 0, AccumulatedRoll_Z_cnt = 0; |
28 | 29 | ||
Line 37... | Line 38... | ||
37 | @post - |
38 | @post - |
38 | @author H. Buss / I. Busker |
39 | @author H. Buss / I. Busker |
39 | **************************************************************************** */ |
40 | **************************************************************************** */ |
40 | void ADC_Init(void) |
41 | void ADC_Init(void) |
41 | { |
42 | { |
- | 43 | #ifdef INTERNAL_REFERENCE |
|
- | 44 | ADMUX = 64;/* Internal Reference 5V */ |
|
- | 45 | #else |
|
42 | ADMUX = 0;//Referenz ist extern |
46 | ADMUX = 0; /* External Reference */ |
- | 47 | #endif |
|
43 | ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE); |
48 | ADCSRA=(1<<ADEN)|(1<<ADSC)|(1<<ADATE)|(1<<ADPS2)|(1<<ADPS1)|(1<<ADPS0)|(1<<ADIE); |
44 | //Free Running Mode, Division Factor 128, Interrupt on |
- | |
45 | } |
49 | } |
Line 46... | Line 50... | ||
46 | 50 | ||
47 | /* **************************************************************************** |
51 | /* **************************************************************************** |
48 | Functionname: SucheLuftruckOffset */ /*! |
52 | Functionname: SucheLuftruckOffset */ /*! |
Line 53... | Line 57... | ||
53 | @post - |
57 | @post - |
54 | @author H. Buss / I. Busker |
58 | @author H. Buss / I. Busker |
55 | **************************************************************************** */ |
59 | **************************************************************************** */ |
56 | void SucheLuftruckOffset(void) |
60 | void SucheLuftruckOffset(void) |
57 | { |
61 | { |
58 | unsigned int off; |
62 | unsigned int off; |
59 | off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]); |
63 | off = eeprom_read_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET]); |
60 | if(off > 20) off -= 10; |
64 | if(off > 20) off -= 10; |
61 | OCR0A = off; |
65 | OCR0A = off; |
62 | Delay_ms_Mess(100); |
66 | Delay_ms_Mess(100); |
63 | if(AdWertAirPressure_Raw < 850) off = 0; |
67 | if(AdWertAirPressure_Raw < 850) off = 0; |
64 | for(; off < 250;off++) |
68 | for(; off < 250;off++) |
65 | { |
69 | { |
66 | OCR0A = off; |
70 | OCR0A = off; |
67 | Delay_ms_Mess(50); |
71 | Delay_ms_Mess(50); |
68 | printf("."); |
72 | printf("."); |
69 | if(AdWertAirPressure_Raw < 900) break; |
73 | if(AdWertAirPressure_Raw < 900) break; |
70 | } |
74 | } |
71 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off); |
75 | eeprom_write_byte(&EEPromArray[EEPROM_ADR_LAST_OFFSET], off); |
72 | Delay_ms_Mess(300); |
76 | Delay_ms_Mess(300); |
73 | } |
77 | } |
Line 74... | Line 78... | ||
74 | 78 | ||
75 | /* **************************************************************************** |
79 | /* **************************************************************************** |
76 | Functionname: SIGNAL */ /*! |
80 | Functionname: SIGNAL */ /*! |
Line 81... | Line 85... | ||
81 | @post - |
85 | @post - |
82 | @author Michael Walter |
86 | @author Michael Walter |
83 | **************************************************************************** */ |
87 | **************************************************************************** */ |
84 | SIGNAL(SIG_ADC) |
88 | SIGNAL(SIG_ADC) |
85 | { |
89 | { |
86 | static unsigned char kanal=0,state = 0; |
90 | static unsigned char kanal=0,state = 0; |
87 | ANALOG_OFF; |
91 | ANALOG_OFF; |
88 | switch(state++) |
92 | switch(state++) |
89 | { |
93 | { |
90 | case 0: |
94 | case 0: |
91 | AdWertGier = ADC; |
95 | AdWertGier = ADC; |
92 | AdWertGier_Raw = AdWertGier; |
96 | AdWertGier_Raw = ADC; |
93 | AccumulatedRoll_Z += (ADC - AdNeutralGier); |
97 | AccumulatedRoll_Z += (ADC - AdNeutralGier); |
94 | AccumulatedRoll_Z_cnt++; |
98 | AccumulatedRoll_Z_cnt++; |
95 | kanal = 1; |
99 | kanal = 1; |
96 | break; |
100 | break; |
97 | case 1: |
101 | case 1: |
98 | AdWertRoll = ADC; |
102 | AdWertRoll = ADC; |
99 | AdWertRoll_Raw = AdWertRoll; |
103 | AdWertRoll_Raw = ADC; |
100 | AccumulatedRoll_X += (ADC - AdNeutralRoll); |
104 | AccumulatedRoll_X += (ADC - AdNeutralRoll); |
101 | AccumulatedRoll_X_cnt++; |
105 | AccumulatedRoll_X_cnt++; |
102 | kanal = 2; |
106 | kanal = 2; |
103 | break; |
107 | break; |
104 | case 2: |
108 | case 2: |
105 | AdWertNick = ADC; |
109 | AdWertNick = ADC; |
106 | AdWertNick_Raw = AdWertNick; |
110 | AdWertNick_Raw = ADC; |
107 | AccumulatedRoll_Y += (ADC - AdNeutralNick); |
111 | AccumulatedRoll_Y += (ADC - AdNeutralNick); |
108 | AccumulatedRoll_Y_cnt++; |
112 | AccumulatedRoll_Y_cnt++; |
109 | kanal = 4; |
113 | kanal = 4; |
110 | break; |
114 | break; |
111 | case 3: |
115 | case 3: |
- | 116 | #ifdef INTERNAL_REFERENCE |
|
- | 117 | UBat = (3 * UBat + (5 * ADC) / 9) / 4; /* The internal Voltage is 5V instesd of 3V */ |
|
- | 118 | #else |
|
112 | UBat = (3 * UBat + ADC / 3) / 4; |
119 | UBat = (3 * UBat + ADC / 3) / 4; |
- | 120 | #endif |
|
113 | kanal = 6; |
121 | kanal = 6; |
114 | break; |
122 | break; |
115 | case 4: |
123 | case 4: |
116 | AdWertAccRoll = NeutralAccY - ADC; |
124 | AdWertAccRoll = NeutralAccY - ADC; |
117 | AdWertAccRoll_Raw = ADC; |
125 | AdWertAccRoll_Raw = ADC; |
118 | AccumulatedACC_Y += (NeutralAccY - ADC); |
126 | AccumulatedACC_Y += (NeutralAccY - ADC); |
119 | AccumulatedACC_Y_cnt++; |
127 | AccumulatedACC_Y_cnt++; |
120 | kanal = 7; |
128 | kanal = 7; |
121 | break; |
129 | break; |
122 | case 5: |
130 | case 5: |
123 | AdWertAccNick = ADC - NeutralAccX; |
131 | AdWertAccNick = ADC - NeutralAccX; |
124 | AdWertAccNick_Raw = ADC; |
132 | AdWertAccNick_Raw = ADC; |
125 | AccumulatedACC_X += (ADC - NeutralAccX); |
133 | AccumulatedACC_X += (ADC - NeutralAccX); |
126 | AccumulatedACC_X_cnt++; |
134 | AccumulatedACC_X_cnt++; |
127 | kanal = 5; |
135 | kanal = 5; |
128 | break; |
136 | break; |
129 | case 6: |
137 | case 6: |
130 | AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2); |
138 | AdWertAccHoch = (ADC - (NeutralAccX + NeutralAccY) / 2); |
131 | AdWertAccHoch_Raw = ADC; |
139 | AdWertAccHoch_Raw = ADC; |
132 | AccumulatedACC_Z += (ADC - NeutralAccZ); |
140 | AccumulatedACC_Z += (ADC - NeutralAccZ); |
133 | AccumulatedACC_Z_cnt++; |
141 | AccumulatedACC_Z_cnt++; |
134 | kanal = 3; |
142 | kanal = 3; |
135 | break; |
143 | break; |
136 | case 7: |
144 | case 7: |
137 | AdWertAirPressure_Raw = ADC; |
145 | AdWertAirPressure_Raw = ADC; |
138 | AccumulatedAirPressure += ADC; |
146 | AccumulatedAirPressure += ADC; |
139 | AccumulatedAirPressure_cnt++; |
147 | AccumulatedAirPressure_cnt++; |
140 | kanal = 0; |
148 | kanal = 0; |
141 | state = 0; |
149 | state = 0; |
142 | break; |
150 | break; |
143 | default: |
151 | default: |
144 | kanal = 0; |
152 | kanal = 0; |
145 | state = 0; |
153 | state = 0; |
146 | break; |
154 | break; |
147 | } |
155 | } |
148 | ADMUX = kanal; |
156 | ADMUX = kanal; |
- | 157 | #ifdef INTERNAL_REFERENCE |
|
- | 158 | /* Add 64 in order to use the internal 5v refenrence */ |
|
- | 159 | ADMUX += 64; |
|
- | 160 | #endif |
|
149 | ANALOG_ON; |
161 | ANALOG_ON; |
150 | } |
162 | } |