Rev 972 | Rev 983 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 972 | Rev 973 | ||
---|---|---|---|
Line 36... | Line 36... | ||
36 | VARIABLES |
36 | VARIABLES |
37 | *****************************************************************************/ |
37 | *****************************************************************************/ |
Line 38... | Line 38... | ||
38 | 38 | ||
Line 39... | Line 39... | ||
39 | extern void GPSupdate(void); |
39 | extern void GPSupdate(void); |
40 | 40 | ||
41 | int AdNeutralNick = 0,AdNeutralRoll = 0,AdNeutralGier = 0; |
41 | f32_t AdNeutralNick = 0.0F, AdNeutralRoll = 0.0F, AdNeutralGier = 0.0F; |
42 | int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0; |
42 | int NeutralAccX = 0, NeutralAccY = 0, NeutralAccZ = 0; |
43 | int AverageRoll_X = 0, AverageRoll_Y = 0, AverageRoll_Z = 0; |
- | |
Line 44... | Line 43... | ||
44 | int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0; |
43 | int AverageRoll = 0, AverageNick = 0, AverageGier = 0; |
45 | int Roll_X_Off = 0, Roll_Y_Off = 0, Roll_Z_Off = 0; |
44 | int AveragerACC_X = 0, AveragerACC_Y = 0, AveragerACC_Z = 0; |
Line 46... | Line 45... | ||
46 | 45 | ||
Line 87... | Line 86... | ||
87 | 86 | ||
88 | LastAltitude = CurrentAltitude; |
87 | LastAltitude = CurrentAltitude; |
Line 89... | Line 88... | ||
89 | Delay_ms_Mess(300); |
88 | Delay_ms_Mess(300); |
90 | 89 | ||
91 | ANALOG_OFF; |
90 | ANALOG_OFF; |
92 | AdNeutralNick = AdWertNick_Raw; |
91 | AdNeutralNick = (f32_t) AdWertNick_Raw; |
93 | AdNeutralRoll = AdWertRoll_Raw; |
92 | AdNeutralRoll = (f32_t) AdWertRoll_Raw; |
94 | AdNeutralGier = AdWertGier_Raw; |
93 | AdNeutralGier = (f32_t) AdWertGier_Raw; |
95 | NeutralAccY = AdWertAccRoll_Raw; |
94 | NeutralAccY = AdWertAccRoll_Raw; |
Line 96... | Line 95... | ||
96 | NeutralAccX = AdWertAccNick_Raw; |
95 | NeutralAccX = AdWertAccNick_Raw; |
Line 108... | Line 107... | ||
108 | AccumulatedACC_Y = 0; |
107 | AccumulatedACC_Y = 0; |
109 | AccumulatedACC_Y_cnt = 0; |
108 | AccumulatedACC_Y_cnt = 0; |
110 | AccumulatedACC_Z = 0; |
109 | AccumulatedACC_Z = 0; |
111 | AccumulatedACC_Z_cnt = 0; |
110 | AccumulatedACC_Z_cnt = 0; |
Line 112... | Line 111... | ||
112 | 111 | ||
113 | AccumulatedRoll_X = 0; |
112 | AccumulatedRoll = 0; |
114 | AccumulatedRoll_X_cnt = 0; |
113 | AccumulatedRoll_cnt = 0; |
115 | AccumulatedRoll_Y = 0; |
114 | AccumulatedNick = 0; |
116 | AccumulatedRoll_Y_cnt = 0; |
115 | AccumulatedNick_cnt = 0; |
117 | AccumulatedRoll_Z = 0; |
116 | AccumulatedGier = 0; |
Line 118... | Line 117... | ||
118 | AccumulatedRoll_Z_cnt = 0; |
117 | AccumulatedGier_cnt = 0; |
119 | 118 | ||
120 | AveragerACC_X = 0; |
119 | AveragerACC_X = 0; |
Line 150... | Line 149... | ||
150 | @author Michael Walter |
149 | @author Michael Walter |
151 | **************************************************************************** */ |
150 | **************************************************************************** */ |
152 | void GetMeasurements(void) |
151 | void GetMeasurements(void) |
153 | { |
152 | { |
154 | ANALOG_OFF; |
153 | ANALOG_OFF; |
155 | AverageRoll_X = AccumulatedRoll_X / AccumulatedRoll_X_cnt; |
154 | AverageRoll = AccumulatedRoll / AccumulatedRoll_cnt; |
156 | AverageRoll_Y = AccumulatedRoll_Y / AccumulatedRoll_Y_cnt; |
155 | AverageNick = AccumulatedNick / AccumulatedNick_cnt; |
157 | AverageRoll_Z = AccumulatedRoll_Z / AccumulatedRoll_Z_cnt; |
156 | AverageGier = AccumulatedGier / AccumulatedGier_cnt; |
Line 158... | Line 157... | ||
158 | 157 | ||
159 | /* Get Pressure Differenz */ |
158 | /* Get Pressure Differenz */ |
160 | CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt; |
159 | CurrentAltitude = InitialAltitude - AccumulatedAirPressure / AccumulatedAirPressure_cnt; |
161 | AccumulatedAirPressure_cnt = 0; |
160 | AccumulatedAirPressure_cnt = 0; |
Line 169... | Line 168... | ||
169 | } |
168 | } |
170 | AirPressureCnt++; |
169 | AirPressureCnt++; |
Line 171... | Line 170... | ||
171 | 170 | ||
172 | //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
171 | //if ((GPS_Roll == 0 && GPS_Nick == 0) || (maxDistance / 10 > 10)) |
173 | { |
172 | { |
174 | Roll_X_Offset = 0.9995F * Roll_X_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_X))); |
173 | AdNeutralNick = 0.999F * AdNeutralNick + 0.001F * AdWertNick_Raw; |
175 | Roll_Y_Offset = 0.9995F * Roll_Y_Offset + 0.0005F * (float) (MAX(-60, MIN(60,AverageRoll_Y))); |
- | |
176 | 174 | AdNeutralRoll = 0.999F * AdNeutralRoll + 0.001F * AdWertRoll_Raw; |
|
177 | if (abs(StickGier) < 15 || MotorenEin == 0) |
175 | if (abs(StickGier) < 15 || MotorenEin == 0) |
178 | { |
176 | { |
179 | Roll_Z_Offset = 0.9998F * Roll_Z_Offset + 0.0002F * (float) ( MAX(-60, MIN(60,AverageRoll_Z))); |
177 | AdNeutralGier = 0.999F * AdNeutralGier + 0.001F * AdWertGier_Raw; |
180 | } |
178 | } |
Line 181... | Line 179... | ||
181 | } |
179 | } |
182 | 180 | ||
183 | #if 1 |
181 | #if 1 |
184 | DebugOut.Analog[7] = AdWertNick_Raw; |
182 | DebugOut.Analog[6] = AdWertNick_Raw; |
- | 183 | DebugOut.Analog[7] = AdWertRoll_Raw; |
|
- | 184 | DebugOut.Analog[8] = AdWertGier_Raw; |
|
- | 185 | ||
- | 186 | DebugOut.Analog[9] = AdNeutralNick; |
|
185 | DebugOut.Analog[8] = AdWertRoll_Raw; |
187 | DebugOut.Analog[10] = AdNeutralRoll; |
Line 186... | Line -... | ||
186 | DebugOut.Analog[9] = AdWertGier_Raw; |
- | |
187 | #endif |
- | |
188 | - | ||
189 | AverageRoll_X -= Roll_X_Offset; |
- | |
190 | AverageRoll_Y -= Roll_Y_Offset; |
188 | DebugOut.Analog[11] = AdNeutralGier; |
191 | AverageRoll_Z -= Roll_Z_Offset; |
189 | #endif |
192 | 190 | ||
193 | AccumulatedRoll_X = 0; |
191 | AccumulatedRoll = 0; |
194 | AccumulatedRoll_X_cnt = 0; |
192 | AccumulatedRoll_cnt = 0; |
195 | AccumulatedRoll_Y = 0; |
193 | AccumulatedNick = 0; |
Line 196... | Line 194... | ||
196 | AccumulatedRoll_Y_cnt = 0; |
194 | AccumulatedNick_cnt = 0; |
197 | AccumulatedRoll_Z = 0; |
195 | AccumulatedGier = 0; |
198 | AccumulatedRoll_Z_cnt = 0; |
196 | AccumulatedGier_cnt = 0; |
199 | 197 | ||
Line 213... | Line 211... | ||
213 | AccumulatedACC_Y_cnt = 0; |
211 | AccumulatedACC_Y_cnt = 0; |
214 | AccumulatedACC_Z = 0; |
212 | AccumulatedACC_Z = 0; |
215 | AccumulatedACC_Z_cnt = 0; |
213 | AccumulatedACC_Z_cnt = 0; |
Line 216... | Line 214... | ||
216 | 214 | ||
217 | ANALOG_ON; |
- | |
218 | - | ||
219 | if (Roll_X_Off > 60) |
- | |
220 | { |
- | |
221 | Roll_X_Off = 60; |
- | |
222 | } |
- | |
223 | if (Roll_X_Off < -60) |
- | |
224 | { |
- | |
225 | Roll_X_Off = -60; |
- | |
226 | } |
- | |
227 | - | ||
228 | if (Roll_Y_Off > 60) |
- | |
229 | { |
- | |
230 | Roll_Y_Off = 60; |
- | |
231 | } |
- | |
232 | if (Roll_Y_Off < -60) |
- | |
233 | { |
- | |
234 | Roll_Y_Off = -60; |
- | |
235 | } |
- | |
236 | - | ||
237 | if (Roll_Z_Off > 60) |
- | |
238 | { |
- | |
239 | Roll_Z_Off = 60; |
- | |
240 | } |
- | |
241 | if (Roll_Z_Off < -60) |
- | |
242 | { |
- | |
243 | Roll_Z_Off = -60; |
- | |
244 | } |
215 | ANALOG_ON; |
Line 245... | Line 216... | ||
245 | } |
216 | } |
246 | 217 | ||
247 | /* **************************************************************************** |
218 | /* **************************************************************************** |
Line 516... | Line 487... | ||
516 | if (abs(StickGier) > 4) |
487 | if (abs(StickGier) > 4) |
517 | { |
488 | { |
518 | sollGier = status.iPsi10 + 4 * StickGier; |
489 | sollGier = status.iPsi10 + 4 * StickGier; |
519 | } |
490 | } |
520 | DiffGier = (sollGier - status.iPsi10); |
491 | DiffGier = (sollGier - status.iPsi10); |
521 | GierMischanteil = (int) (-4 * DiffGier - 4* (AdWertGier - AdNeutralGier - Roll_Z_Off)) / 10; |
492 | GierMischanteil = (int) (-4 * DiffGier - 4* (AdWertGier_Raw - (int) AdNeutralGier)) / 10; |
Line 522... | Line 493... | ||
522 | 493 | ||
523 | #define MUL_G 0.8 |
494 | #define MUL_G 0.8 |
524 | if(GierMischanteil > MUL_G * GasMischanteil) |
495 | if(GierMischanteil > MUL_G * GasMischanteil) |
525 | { |
496 | { |
Line 576... | Line 547... | ||
576 | if(SummeRoll < -10000) |
547 | if(SummeRoll < -10000) |
577 | { |
548 | { |
578 | SummeRoll = -10000; |
549 | SummeRoll = -10000; |
579 | } |
550 | } |
Line 580... | Line 551... | ||
580 | 551 | ||
581 | pd_ergebnis = ((scale_p *DiffNick + scale_d * (AdWertNick - AdNeutralNick - Roll_Y_Off)) / 10) ; // + (int)(SummeNick / 2000); |
552 | pd_ergebnis = ((scale_p *DiffNick + scale_d * (AdWertNick_Raw - (int) AdNeutralNick)) / 10) ; // + (int)(SummeNick / 2000); |
582 | if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
553 | if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
583 | { |
554 | { |
584 | pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
555 | pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
585 | } |
556 | } |
Line 618... | Line 589... | ||
618 | { |
589 | { |
619 | motorwert = MIN_GAS; |
590 | motorwert = MIN_GAS; |
620 | } |
591 | } |
621 | Motor_Hinten = motorwert; |
592 | Motor_Hinten = motorwert; |
Line 622... | Line 593... | ||
622 | 593 | ||
623 | pd_ergebnis = ((scale_p * DiffRoll + scale_d * (AdWertRoll - AdNeutralRoll - Roll_X_Off)) / 10) ; //+ (int)(SummeRoll / 2000); |
594 | pd_ergebnis = ((scale_p * DiffRoll + scale_d * (AdWertRoll_Raw - (int) AdNeutralRoll)) / 10) ; //+ (int)(SummeRoll / 2000); |
624 | if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
595 | if(pd_ergebnis > (GasMischanteil + abs(GierMischanteil))) |
625 | { |
596 | { |
626 | pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
597 | pd_ergebnis = (GasMischanteil + abs(GierMischanteil)); |
627 | } |
598 | } |