Rev 851 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 851 | Rev 857 | ||
---|---|---|---|
Line 83... | Line 83... | ||
83 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
83 | long Umschlag180Nick = 250000L, Umschlag180Roll = 250000L; |
Line 84... | Line 84... | ||
84 | 84 | ||
85 | float GyroFaktor; |
85 | float GyroFaktor; |
86 | float IntegralFaktor; |
86 | float IntegralFaktor; |
87 | int DiffNick,DiffRoll; |
87 | int DiffNick,DiffRoll; |
88 | int Poti1 = 0, Poti2 = 0, Poti3 = 0, Poti4 = 0; |
88 | uint8_t Poti1, Poti2, Poti3, Poti4; |
89 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
89 | volatile unsigned char Motor_Vorne,Motor_Hinten,Motor_Rechts,Motor_Links, Count; |
90 | unsigned char MotorWert[5]; |
90 | unsigned char MotorWert[5]; |
91 | volatile unsigned char SenderOkay = 0; |
91 | volatile unsigned char SenderOkay = 0; |
92 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
92 | int StickNick = 0,StickRoll = 0,StickGier = 0,StickGas = 0; |
Line 118... | Line 118... | ||
118 | unsigned char Parameter_ServoNickControl = 100; |
118 | unsigned char Parameter_ServoNickControl = 100; |
119 | unsigned char Parameter_LoopGasLimit = 70; |
119 | unsigned char Parameter_LoopGasLimit = 70; |
120 | unsigned char Parameter_AchsKopplung1 = 0; |
120 | unsigned char Parameter_AchsKopplung1 = 0; |
121 | unsigned char Parameter_AchsGegenKopplung1 = 0; |
121 | unsigned char Parameter_AchsGegenKopplung1 = 0; |
122 | unsigned char Parameter_DynamicStability = 100; |
122 | unsigned char Parameter_DynamicStability = 100; |
- | 123 | uint8_t Cam_Travel; |
|
- | 124 | int8_t nick_tilt, roll_tilt; |
|
Line 123... | Line 125... | ||
123 | 125 | ||
124 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
126 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
Line 125... | Line 127... | ||
125 | int MaxStickNick = 0,MaxStickRoll = 0; |
127 | int MaxStickNick = 0,MaxStickRoll = 0; |
Line 173... | Line 175... | ||
173 | 175 | ||
174 | //############################################################################ |
176 | //############################################################################ |
175 | // Nullwerte ermitteln |
177 | // Nullwerte ermitteln |
176 | void SetNeutral(void) |
178 | void SetNeutral(void) |
177 | //############################################################################ |
179 | //############################################################################ |
178 | { |
180 | { |
179 | acc_neutral.X = 0; |
181 | acc_neutral.X = 0; |
180 | acc_neutral.Y = 0; |
182 | acc_neutral.Y = 0; |
181 | acc_neutral.Z = 0; |
183 | acc_neutral.Z = 0; |
182 | AdNeutralNick = 0; |
184 | AdNeutralNick = 0; |
Line 210... | Line 212... | ||
210 | MesswertGier = 0; |
212 | MesswertGier = 0; |
211 | StartLuftdruck = Luftdruck; |
213 | StartLuftdruck = Luftdruck; |
212 | HoeheD = 0; |
214 | HoeheD = 0; |
213 | Mess_Integral_Hoch = 0; |
215 | Mess_Integral_Hoch = 0; |
214 | KompassStartwert = KompassValue; |
216 | KompassStartwert = KompassValue; |
215 | GPS_Neutral(); |
- | |
216 | beeptime = 50; |
217 | beeptime = 50; |
217 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
218 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
218 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
219 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagRoll * 2500L; |
219 | ExternHoehenValue = 0; |
220 | ExternHoehenValue = 0; |
- | 221 | Cam_Travel = EE_Parameter.ServoNickMax - EE_Parameter.ServoNickMin; |
|
220 | } |
222 | } |
Line 221... | Line 223... | ||
221 | 223 | ||
222 | //############################################################################ |
224 | //############################################################################ |
223 | // Bearbeitet die Messwerte |
225 | // Bearbeitet die Messwerte |
Line 268... | Line 270... | ||
268 | Mess_IntegralRoll = (Umschlag180Roll - 10000L); |
270 | Mess_IntegralRoll = (Umschlag180Roll - 10000L); |
269 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
271 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
270 | } |
272 | } |
271 | if(AdWertRoll < 15) MesswertRoll = -1000; |
273 | if(AdWertRoll < 15) MesswertRoll = -1000; |
272 | if(AdWertRoll < 7) MesswertRoll = -2000; |
274 | if(AdWertRoll < 7) MesswertRoll = -2000; |
273 | if(PlatinenVersion == 10) |
275 | #if (PlatinenVersion == 10) |
274 | { |
276 | { |
275 | if(AdWertRoll > 1010) MesswertRoll = +1000; |
277 | if(AdWertRoll > 1010) MesswertRoll = +1000; |
276 | if(AdWertRoll > 1017) MesswertRoll = +2000; |
278 | if(AdWertRoll > 1017) MesswertRoll = +2000; |
277 | } |
279 | } |
278 | else |
280 | #else |
279 | { |
281 | { |
280 | if(AdWertRoll > 2020) MesswertRoll = +1000; |
282 | if(AdWertRoll > 2020) MesswertRoll = +1000; |
281 | if(AdWertRoll > 2034) MesswertRoll = +2000; |
283 | if(AdWertRoll > 2034) MesswertRoll = +2000; |
282 | } |
284 | } |
- | 285 | #endif |
|
283 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
286 | // Nick ++++++++++++++++++++++++++++++++++++++++++++++++ |
284 | MesswertNick -= tmpl2; |
287 | MesswertNick -= tmpl2; |
285 | MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
288 | MesswertNick -= (tmpl*Parameter_AchsGegenKopplung1)/512L; |
286 | Mess_IntegralNick2 += MesswertNick; |
289 | Mess_IntegralNick2 += MesswertNick; |
287 | Mess_IntegralNick += MesswertNick - LageKorrekturNick; |
290 | Mess_IntegralNick += MesswertNick - LageKorrekturNick; |
Line 295... | Line 298... | ||
295 | Mess_IntegralNick = (Umschlag180Nick - 10000L); |
298 | Mess_IntegralNick = (Umschlag180Nick - 10000L); |
296 | Mess_IntegralNick2 = Mess_IntegralNick; |
299 | Mess_IntegralNick2 = Mess_IntegralNick; |
297 | } |
300 | } |
298 | if(AdWertNick < 15) MesswertNick = -1000; |
301 | if(AdWertNick < 15) MesswertNick = -1000; |
299 | if(AdWertNick < 7) MesswertNick = -2000; |
302 | if(AdWertNick < 7) MesswertNick = -2000; |
300 | if(PlatinenVersion == 10) |
303 | #if(PlatinenVersion == 10) |
301 | { |
304 | { |
302 | if(AdWertNick > 1010) MesswertNick = +1000; |
305 | if(AdWertNick > 1010) MesswertNick = +1000; |
303 | if(AdWertNick > 1017) MesswertNick = +2000; |
306 | if(AdWertNick > 1017) MesswertNick = +2000; |
304 | } |
307 | } |
305 | else |
308 | #else |
306 | { |
309 | { |
307 | if(AdWertNick > 2020) MesswertNick = +1000; |
310 | if(AdWertNick > 2020) MesswertNick = +1000; |
308 | if(AdWertNick > 2034) MesswertNick = +2000; |
311 | if(AdWertNick > 2034) MesswertNick = +2000; |
309 | } |
312 | } |
- | 313 | #endif |
|
310 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
314 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
311 | // ADC einschalten |
315 | // ADC einschalten |
312 | ANALOG_ON; |
316 | ANALOG_ON; |
313 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
317 | //++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 323... | Line 327... | ||
323 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
327 | if(MesswertNick > 200) MesswertNick += 4 * (MesswertNick - 200); |
324 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
328 | else if(MesswertNick < -200) MesswertNick += 4 * (MesswertNick + 200); |
325 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
329 | if(MesswertRoll > 200) MesswertRoll += 4 * (MesswertRoll - 200); |
326 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
330 | else if(MesswertRoll < -200) MesswertRoll += 4 * (MesswertRoll + 200); |
327 | } |
331 | } |
- | 332 | ||
328 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
333 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--; |
329 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
334 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--; |
330 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
335 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--; |
331 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
336 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--; |
332 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
- | |
333 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
- | |
334 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
- | |
335 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
- | |
336 | } |
337 | } |
Line 337... | Line 338... | ||
337 | 338 | ||
338 | //############################################################################ |
339 | //############################################################################ |
339 | // Messwerte beim Ermitteln der Nullage |
340 | // Messwerte beim Ermitteln der Nullage |
Line 347... | Line 348... | ||
347 | MesswertGier = AdWertGier; |
348 | MesswertGier = AdWertGier; |
348 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
349 | Mittelwert_AccNick = ACC_AMPLIFY * (long)AdWertAccNick; |
349 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
350 | Mittelwert_AccRoll = ACC_AMPLIFY * (long)AdWertAccRoll; |
350 | // ADC einschalten |
351 | // ADC einschalten |
351 | ANALOG_ON; |
352 | ANALOG_ON; |
352 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 110 && Poti1) Poti1--; |
353 | if(Poti1 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 < 255) Poti1++; else if(Poti1 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI1]] + 128 && Poti1 > 0) Poti1--; |
353 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 110 && Poti2) Poti2--; |
354 | if(Poti2 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 < 255) Poti2++; else if(Poti2 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI2]] + 128 && Poti2 > 0) Poti2--; |
354 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 110 && Poti3) Poti3--; |
355 | if(Poti3 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 < 255) Poti3++; else if(Poti3 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI3]] + 128 && Poti3 > 0) Poti3--; |
355 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 110 && Poti4) Poti4--; |
356 | if(Poti4 < PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 < 255) Poti4++; else if(Poti4 > PPM_in[EE_Parameter.Kanalbelegung[K_POTI4]] + 128 && Poti4 > 0) Poti4--; |
356 | if(Poti1 < 0) Poti1 = 0; else if(Poti1 > 255) Poti1 = 255; |
- | |
357 | if(Poti2 < 0) Poti2 = 0; else if(Poti2 > 255) Poti2 = 255; |
- | |
358 | if(Poti3 < 0) Poti3 = 0; else if(Poti3 > 255) Poti3 = 255; |
- | |
359 | if(Poti4 < 0) Poti4 = 0; else if(Poti4 > 255) Poti4 = 255; |
- | |
360 | - | ||
361 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
357 | Umschlag180Nick = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
362 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
358 | Umschlag180Roll = (long) EE_Parameter.WinkelUmschlagNick * 2500L; |
363 | } |
359 | } |
Line 364... | Line 360... | ||
364 | 360 | ||
Line 497... | Line 493... | ||
497 | PPM_in_gier = 0; |
493 | PPM_in_gier = 0; |
498 | } |
494 | } |
499 | else MotorenEin = 0; |
495 | else MotorenEin = 0; |
500 | } |
496 | } |
501 | else |
497 | else |
- | 498 | ||
502 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
499 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
503 | // Emfang gut |
500 | // Emfang gut |
504 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
501 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
505 | if(SenderOkay > 140) |
502 | if(SenderOkay > 140) |
506 | { |
503 | { |
Line 517... | Line 514... | ||
517 | Mess_Integral_Gier = 0; |
514 | Mess_Integral_Gier = 0; |
518 | Mess_Integral_Gier2 = 0; |
515 | Mess_Integral_Gier2 = 0; |
519 | } |
516 | } |
520 | if((PPM_in_gas > 80) && MotorenEin == 0) |
517 | if((PPM_in_gas > 80) && MotorenEin == 0) |
521 | { |
518 | { |
- | 519 | ||
522 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
520 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
523 | // auf Nullwerte kalibrieren |
521 | // auf Nullwerte kalibrieren |
524 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
522 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
525 | if(PPM_in_gier > 75) // Neutralwerte |
523 | if(PPM_in_gier > 75) // Neutralwerte |
526 | { |
524 | { |
Line 545... | Line 543... | ||
545 | Piep(GetActiveParamSetNumber()); |
543 | Piep(GetActiveParamSetNumber()); |
546 | } |
544 | } |
547 | } |
545 | } |
548 | else delay_neutral = 0; |
546 | else delay_neutral = 0; |
549 | } |
547 | } |
- | 548 | ||
550 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
549 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
551 | // Gas ist unten |
550 | // Gas ist unten |
552 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
551 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
553 | if(PPM_in_gas < 35-120) |
552 | if(PPM_in_gas < -85) |
554 | { |
553 | { |
555 | // Starten |
554 | // Starten |
556 | if(PPM_in_gier < -75) |
555 | if(PPM_in_gier < -75) |
557 | { |
556 | { |
- | 557 | ||
558 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
558 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
559 | // Einschalten |
559 | // Einschalten |
560 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
560 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
561 | if(++delay_einschalten > 200) |
561 | if(++delay_einschalten > 200) |
562 | { |
562 | { |
Line 574... | Line 574... | ||
574 | SummeRoll = 0; |
574 | SummeRoll = 0; |
575 | } |
575 | } |
576 | } |
576 | } |
577 | else delay_einschalten = 0; |
577 | else delay_einschalten = 0; |
578 | //Auf Neutralwerte setzen |
578 | //Auf Neutralwerte setzen |
- | 579 | ||
579 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
580 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
580 | // Auschalten |
581 | // Auschalten |
581 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
582 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
582 | if(PPM_in_gier > 75) |
583 | if(PPM_in_gier > 75) |
583 | { |
584 | { |
Line 612... | Line 613... | ||
612 | if(abs(PPM_in_roll) > MaxStickRoll) |
613 | if(abs(PPM_in_roll) > MaxStickRoll) |
613 | MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--; |
614 | MaxStickRoll = abs(PPM_in_roll); else MaxStickRoll--; |
614 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
615 | if(Notlandung) {MaxStickNick = 0; MaxStickRoll = 0;} |
Line 615... | Line 616... | ||
615 | 616 | ||
616 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
617 | GyroFaktor = ((float)Parameter_Gyro_P + 10.0) / 256.0; |
Line 617... | Line 618... | ||
617 | IntegralFaktor = ((float) Parameter_Gyro_I) / 44000; |
618 | IntegralFaktor = ((float)Parameter_Gyro_I) / 44000; |
618 | 619 | ||
619 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
620 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
620 | //+ Digitale Steuerung per DubWise |
621 | //+ Digitale Steuerung per DubWise |
Line 634... | Line 635... | ||
634 | if(DubWiseKeys[0] & 16) ExternHoehenValue--; |
635 | if(DubWiseKeys[0] & 16) ExternHoehenValue--; |
Line 635... | Line 636... | ||
635 | 636 | ||
636 | StickNick += ExternStickNick / 8; |
637 | StickNick += ExternStickNick / 8; |
637 | StickRoll += ExternStickRoll / 8; |
638 | StickRoll += ExternStickRoll / 8; |
- | 639 | StickGier += ExternStickGier; |
|
638 | StickGier += ExternStickGier; |
640 | |
639 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
641 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
640 | //+ Analoge Steuerung per Seriell |
642 | //+ Analoge Steuerung per Seriell |
641 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
643 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
642 | if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128) |
644 | if(ExternControl.Config & 0x01 && Parameter_UserParam1 > 128) |
Line 649... | Line 651... | ||
649 | } |
651 | } |
Line 650... | Line 652... | ||
650 | 652 | ||
651 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
653 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
652 | if(GyroFaktor < 0) GyroFaktor = 0; |
654 | if(GyroFaktor < 0) GyroFaktor = 0; |
- | 655 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
|
653 | if(IntegralFaktor < 0) IntegralFaktor = 0; |
656 | |
654 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
657 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
655 | // Looping? |
658 | // Looping? |
656 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
659 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
657 | if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
660 | if((PPM_in_roll > EE_Parameter.LoopThreshold) && EE_Parameter.LoopConfig & CFG_LOOP_LINKS) Looping_Links = 1; |
Line 710... | Line 713... | ||
710 | IntegralFaktor = 0.003; |
713 | IntegralFaktor = 0.003; |
711 | Looping_Roll = 0; |
714 | Looping_Roll = 0; |
712 | Looping_Nick = 0; |
715 | Looping_Nick = 0; |
713 | } |
716 | } |
Line 714... | Line -... | ||
714 | - | ||
715 | 717 | ||
716 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
718 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
717 | // Integrale auf ACC-Signal abgleichen |
719 | // Integrale auf ACC-Signal abgleichen |
718 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
720 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 749... | Line 751... | ||
749 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25)) |
751 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in_gier) > 25)) |
750 | { |
752 | { |
751 | tmp_long /= 3; |
753 | tmp_long /= 3; |
752 | tmp_long2 /= 3; |
754 | tmp_long2 /= 3; |
753 | } |
755 | } |
754 | //if(abs(PPM_in_gier) > 25) |
- | |
755 | // { |
- | |
756 | // tmp_long /= 3; |
- | |
757 | // tmp_long2 /= 3; |
- | |
758 | // } |
- | |
Line 759... | Line 756... | ||
759 | 756 | ||
760 | #define AUSGLEICH 32 |
757 | #define AUSGLEICH 32 |
761 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
758 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
762 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
759 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
763 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
760 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
Line 921... | Line 918... | ||
921 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
918 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
Line 922... | Line 919... | ||
922 | 919 | ||
923 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
920 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
924 | // Gieren |
921 | // Gieren |
925 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
922 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
926 | if(abs(StickGier) > 20) // war 35 |
923 | if(abs(StickGier) > 20) |
927 | { |
924 | { |
928 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
925 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
929 | } |
926 | } |
930 | tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
927 | tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
Line 937... | Line 934... | ||
937 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
934 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
938 | // Kompass |
935 | // Kompass |
939 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
936 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
940 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
937 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
941 | { |
938 | { |
942 | if (!updKompass--) // Aufruf mit ~10 Hz |
939 | if (!updKompass--) // Aufruf mit ~15 Hz |
943 | { |
940 | { |
944 | updKompass = 49; |
941 | updKompass = 33; |
Line 945... | Line 942... | ||
945 | 942 | ||
946 | if ((MaxStickNick < 50) && (MaxStickRoll < 50)) // Bei extremen Flugmanövern keine Kompassauswertung |
943 | if ((MaxStickNick < 50) && (MaxStickRoll < 50)) // Bei extremen Flugmanövern keine Kompassauswertung |
947 | { |
944 | { |
948 | KompassValue = heading_MM3(); |
945 | KompassValue = heading_MM3(); |
Line 961... | Line 958... | ||
961 | // Debugwerte zuordnen |
958 | // Debugwerte zuordnen |
962 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
959 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
963 | if(!TimerWerteausgabe--) |
960 | if(!TimerWerteausgabe--) |
964 | { |
961 | { |
965 | TimerWerteausgabe = 24; |
962 | TimerWerteausgabe = 24; |
966 | - | ||
967 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
963 | //DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
968 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
964 | //DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
969 | DebugOut.Analog[2] = Mittelwert_AccNick; |
965 | DebugOut.Analog[2] = Mittelwert_AccNick; |
970 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
966 | DebugOut.Analog[3] = Mittelwert_AccRoll; |
971 | DebugOut.Analog[4] = MesswertGier; |
967 | DebugOut.Analog[4] = MesswertGier; |
972 | DebugOut.Analog[5] = HoehenWert; |
968 | DebugOut.Analog[5] = HoehenWert; |
973 | DebugOut.Analog[6] = Mess_Integral_Hoch / 512; |
969 | DebugOut.Analog[6] = Mess_Integral_Hoch / 512; |
Line 1091... | Line 1087... | ||
1091 | 1087 | ||
1092 | if(GasMischanteil < 20) GierMischanteil = 0; |
1088 | if(GasMischanteil < 20) GierMischanteil = 0; |
1093 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1089 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1094 | // Nick-Achse |
1090 | // Nick-Achse |
1095 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1091 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1096 | DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
1092 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1097 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
1093 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung |
1098 | else SummeNick += DiffNick; // I-Anteil bei HH |
1094 | else SummeNick += DiffNick; // I-Anteil bei HH |
1099 | if(SummeNick > 16000) SummeNick = 16000; |
1095 | if(SummeNick > 16000) SummeNick = 16000; |
1100 | if(SummeNick < -16000) SummeNick = -16000; |
1096 | if(SummeNick < -16000) SummeNick = -16000; |
1101 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
1097 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
Line 1116... | Line 1112... | ||
1116 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1112 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1117 | Motor_Hinten = motorwert; |
1113 | Motor_Hinten = motorwert; |
1118 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1114 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1119 | // Roll-Achse |
1115 | // Roll-Achse |
1120 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1116 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1121 | DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1117 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1122 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1118 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1123 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1119 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1124 | if(SummeRoll > 16000) SummeRoll = 16000; |
1120 | if(SummeRoll > 16000) SummeRoll = 16000; |
1125 | if(SummeRoll < -16000) SummeRoll = -16000; |
1121 | if(SummeRoll < -16000) SummeRoll = -16000; |
1126 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1122 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1127 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1123 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1128 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1124 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1129 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1125 | if(pd_ergebnis < -tmp_int) pd_ergebnis = -tmp_int; |
1130 | // Motor Links |
1126 | // Motor Links |
1131 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
1127 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
1132 | #define GRENZE Poti1 |
- | |
Line 1133... | Line 1128... | ||
1133 | 1128 | ||
1134 | if ((motorwert < 0)) motorwert = 0; |
1129 | if ((motorwert < 0)) motorwert = 0; |
1135 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
1130 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
1136 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1131 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |