Rev 1793 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1793 | Rev 1797 | ||
---|---|---|---|
Line 71... | Line 71... | ||
71 | long IntegralRoll = 0,IntegralRoll2 = 0; |
71 | long IntegralRoll = 0,IntegralRoll2 = 0; |
72 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
72 | long IntegralAccNick = 0,IntegralAccRoll = 0,IntegralAccZ = 0; |
73 | long Integral_Gier = 0; |
73 | long Integral_Gier = 0; |
74 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
74 | long Mess_IntegralNick = 0,Mess_IntegralNick2 = 0; |
75 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
75 | long Mess_IntegralRoll = 0,Mess_IntegralRoll2 = 0; |
76 | long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; |
76 | //long Mess_Integral_Gier = 0,Mess_Integral_Gier2 = 0; //MartinR so war es |
- | 77 | long Mess_Integral_Gier = 0; //MartinR: Mess_Integral_Gier2 unbenutzt |
|
77 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
78 | long MittelIntegralNick,MittelIntegralRoll,MittelIntegralNick2,MittelIntegralRoll2; |
78 | long SummeNick=0,SummeRoll=0; |
79 | long SummeNick=0,SummeRoll=0; |
79 | volatile long Mess_Integral_Hoch = 0; |
80 | volatile long Mess_Integral_Hoch = 0; |
80 | int KompassValue = 0; |
81 | int KompassValue = 0; |
81 | int KompassStartwert = 0; |
82 | int KompassStartwert = 0; |
Line 99... | Line 100... | ||
99 | long HoehenWert = 0; |
100 | long HoehenWert = 0; |
100 | long SollHoehe = 0; |
101 | long SollHoehe = 0; |
101 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
102 | int LageKorrekturRoll = 0,LageKorrekturNick = 0, HoverGas = 0; |
102 | //float Ki = FAKTOR_I; |
103 | //float Ki = FAKTOR_I; |
103 | int Ki = 10300 / 33; |
104 | int Ki = 10300 / 33; |
- | 105 | int KiHH = 10300 / 33; // MartinR : für Ki bei HH über Schalter |
|
- | 106 | ||
104 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
107 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
105 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
108 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
Line 106... | Line 109... | ||
106 | 109 | ||
107 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
110 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
Line 150... | Line 153... | ||
150 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
153 | unsigned char Parameter_Servo3,Parameter_Servo4,Parameter_Servo5; |
151 | unsigned char CareFree = 0; |
154 | unsigned char CareFree = 0; |
152 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; |
155 | const signed char sintab[31] = { 0, 2, 4, 6, 7, 8, 8, 8, 7, 6, 4, 2, 0, -2, -4, -6, -7, -8, -8, -8, -7, -6, -4, -2, 0, 2, 4, 6, 7, 8, 8}; |
Line 153... | Line 156... | ||
153 | 156 | ||
154 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
157 | signed int ExternStickNick = 0,ExternStickRoll = 0,ExternStickGier = 0, ExternHoehenValue = -20; |
- | 158 | //int MaxStickNick = 0,MaxStickRoll = 0; MartinR: so war es |
|
- | 159 | int MaxStickNick = 0,MaxStickRoll = 0,stick_nick_neutral = 0,stick_roll_neutral = 0; // MartinR: stick_.._neutral hinzugefügt |
|
155 | int MaxStickNick = 0,MaxStickRoll = 0; |
160 | |
156 | unsigned int modell_fliegt = 0; |
161 | unsigned int modell_fliegt = 0; |
157 | volatile unsigned char FC_StatusFlags = 0; |
162 | volatile unsigned char FC_StatusFlags = 0; |
158 | long GIER_GRAD_FAKTOR = 1291; |
163 | long GIER_GRAD_FAKTOR = 1291; |
159 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
164 | signed int KopplungsteilNickRoll,KopplungsteilRollNick; |
Line 183... | Line 188... | ||
183 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
188 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
184 | DebugOut.Analog[12] = Motor[0].SetPoint; |
189 | DebugOut.Analog[12] = Motor[0].SetPoint; |
185 | DebugOut.Analog[13] = Motor[1].SetPoint; |
190 | DebugOut.Analog[13] = Motor[1].SetPoint; |
186 | DebugOut.Analog[14] = Motor[2].SetPoint; |
191 | DebugOut.Analog[14] = Motor[2].SetPoint; |
187 | DebugOut.Analog[15] = Motor[3].SetPoint; |
192 | DebugOut.Analog[15] = Motor[3].SetPoint; |
- | 193 | //DebugOut.Analog[16] = DiffNick; // MartinR: test |
|
- | 194 | //DebugOut.Analog[17] = DiffRoll; // MartinR: test |
|
- | 195 | //DebugOut.Analog[18] = MesswertNick; // MartinR: test |
|
- | 196 | //DebugOut.Analog[19] = MesswertRoll; // MartinR: test |
|
188 | DebugOut.Analog[20] = ServoNickValue; |
197 | DebugOut.Analog[20] = ServoNickValue; |
189 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
198 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
190 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
199 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
191 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
200 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
192 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
201 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
Line 196... | Line 205... | ||
196 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
205 | if(VersionInfo.HardwareError[0] || VersionInfo.HardwareError[1]) DebugOut.Status[1] |= 1; else DebugOut.Status[1] &= 0xfe; |
Line 197... | Line 206... | ||
197 | 206 | ||
198 | //if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/) { beeptime = 1000; DebugOut.Analog[25]++; } |
207 | //if(Capacity.MinOfMaxPWM < 250/* && modell_fliegt > 500*/) { beeptime = 1000; DebugOut.Analog[25]++; } |
Line 199... | Line -... | ||
199 | } |
- | |
200 | - | ||
201 | 208 | } |
|
202 | 209 | ||
203 | void Piep(unsigned char Anzahl, unsigned int dauer) |
210 | void Piep(unsigned char Anzahl, unsigned int dauer) |
204 | { |
211 | { |
205 | if(MotorenEin) return; //auf keinen Fall im Flug! |
212 | if(MotorenEin) return; //auf keinen Fall im Flug! |
Line 366... | Line 373... | ||
366 | //############################################################################ |
373 | //############################################################################ |
367 | { |
374 | { |
368 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
375 | static signed long tmpl,tmpl2,tmpl3,tmpl4; |
369 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
376 | static signed int oldNick, oldRoll, d2Roll, d2Nick; |
370 | signed long winkel_nick, winkel_roll; |
377 | signed long winkel_nick, winkel_roll; |
371 | unsigned char i; |
378 | //unsigned char i; // MartinR: wird bei V0.81d nicht mehr benötigt, da Poti-Auswertung verschoben |
372 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
379 | MesswertGier = (signed int) AdNeutralGier - AdWertGier; |
373 | MesswertNick = (signed int) AdWertNickFilter / 8; |
380 | //MesswertNick = (signed int) AdWertNickFilter / 8; // MartinR: so war es |
374 | MesswertRoll = (signed int) AdWertRollFilter / 8; |
381 | //MesswertRoll = (signed int) AdWertRollFilter / 8; // MartinR: so war es |
- | 382 | MesswertNick = (signed int) AdWertNickFilter ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
- | 383 | MesswertRoll = (signed int) AdWertRollFilter ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
375 | RohMesswertNick = MesswertNick; |
384 | RohMesswertNick = MesswertNick; |
376 | RohMesswertRoll = MesswertRoll; |
385 | RohMesswertRoll = MesswertRoll; |
Line 377... | Line 386... | ||
377 | 386 | ||
378 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
387 | // Beschleunigungssensor ++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 463... | Line 472... | ||
463 | IntegralNick = Mess_IntegralNick; |
472 | IntegralNick = Mess_IntegralNick; |
464 | IntegralRoll = Mess_IntegralRoll; |
473 | IntegralRoll = Mess_IntegralRoll; |
465 | IntegralNick2 = Mess_IntegralNick2; |
474 | IntegralNick2 = Mess_IntegralNick2; |
466 | IntegralRoll2 = Mess_IntegralRoll2; |
475 | IntegralRoll2 = Mess_IntegralRoll2; |
Line -... | Line 476... | ||
- | 476 | ||
467 | 477 | //#define D_LIMIT 128 // MartinR: so war es |
|
- | 478 | #define D_LIMIT 16 |
|
468 | #define D_LIMIT 128 |
479 | // MartinR: Änderung war notwendig, da die Division /8 bereits in der analog.c erfolgt |
469 | 480 | ||
470 | MesswertNick = HiResNick / 8; |
481 | //MesswertNick = HiResNick / 8; // MartinR : so war es |
- | 482 | // MesswertRoll = HiResRoll / 8; // MartinR : so war es |
|
- | 483 | MesswertNick = HiResNick ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
Line -... | Line 484... | ||
- | 484 | MesswertRoll = HiResRoll ; // MartinR die Division /8 erfolgt bereits in der analog.c |
|
- | 485 | ||
- | 486 | // MartinR : so war es Anfang |
|
471 | MesswertRoll = HiResRoll / 8; |
487 | /* |
472 | 488 | |
|
473 | if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; |
489 | if(AdWertNick < 15) MesswertNick = -1000; if(AdWertNick < 7) MesswertNick = -2000; |
474 | if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } |
490 | if(PlatinenVersion == 10) { if(AdWertNick > 1010) MesswertNick = +1000; if(AdWertNick > 1017) MesswertNick = +2000; } |
475 | else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
491 | else { if(AdWertNick > 2000) MesswertNick = +1000; if(AdWertNick > 2015) MesswertNick = +2000; } |
476 | if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
492 | if(AdWertRoll < 15) MesswertRoll = -1000; if(AdWertRoll < 7) MesswertRoll = -2000; |
- | 493 | if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
|
- | 494 | else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
|
- | 495 | |
|
- | 496 | // MartinR : FC 1.0: Sprung von 500 auf 2000 !! FC-ME: Sprung von 1000 auf 2000 |
|
- | 497 | */ |
|
- | 498 | // MartinR : so war es Ende |
|
- | 499 | ||
- | 500 | // MartinR : Neu Anfang |
|
- | 501 | if(PlatinenVersion == 10) |
|
- | 502 | { |
|
- | 503 | if(AdWertNick > 1010) MesswertNick = +600; |
|
- | 504 | if(AdWertNick > 1017) MesswertNick = +800; |
|
- | 505 | if(AdWertNick < 15) MesswertNick = -600; |
|
- | 506 | if(AdWertNick < 7) MesswertNick = -800; |
|
- | 507 | if(AdWertRoll > 1010) MesswertRoll = +600; |
|
- | 508 | if(AdWertRoll > 1017) MesswertRoll = +800; |
|
- | 509 | if(AdWertRoll < 15) MesswertRoll = -600; |
|
- | 510 | if(AdWertRoll < 7) MesswertRoll = -800; |
|
- | 511 | } |
|
- | 512 | else |
|
- | 513 | { |
|
- | 514 | if(AdWertNick > 2000) MesswertNick = +1200; |
|
- | 515 | if(AdWertNick > 2015) MesswertNick = +1600; |
|
- | 516 | if(AdWertNick < 15) MesswertNick = -1200; |
|
- | 517 | if(AdWertNick < 7) MesswertNick = -1600; |
|
- | 518 | if(AdWertRoll > 2000) MesswertRoll = +1200; |
|
- | 519 | if(AdWertRoll > 2015) MesswertRoll = +1600; |
|
- | 520 | if(AdWertRoll < 15) MesswertRoll = -1200; |
|
- | 521 | if(AdWertRoll < 7) MesswertRoll = -1600; |
|
Line 477... | Line 522... | ||
477 | if(PlatinenVersion == 10) { if(AdWertRoll > 1010) MesswertRoll = +1000; if(AdWertRoll > 1017) MesswertRoll = +2000; } |
522 | } |
- | 523 | // MartinR : Neu Ende |
|
- | 524 | ||
478 | else { if(AdWertRoll > 2000) MesswertRoll = +1000; if(AdWertRoll > 2015) MesswertRoll = +2000; } |
525 | if(Parameter_Gyro_D) |
479 | 526 | // MartinR: hier sind Änderungen erforderlich, da u.a. MesswertNick = HiResNick / 8 von der fc.c in die analog.c verschoben wurde |
|
480 | if(Parameter_Gyro_D) |
527 | // Hintergrund: Code einsparen |
481 | { |
528 | { |
482 | d2Nick = HiResNick - oldNick; |
529 | d2Nick = HiResNick - oldNick; |
483 | oldNick = (oldNick + HiResNick)/2; |
- | |
484 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
530 | oldNick = (oldNick + HiResNick)/2; |
485 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
531 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
486 | 532 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
|
487 | d2Roll = HiResRoll - oldRoll; |
533 | d2Roll = HiResRoll - oldRoll; |
488 | oldRoll = (oldRoll + HiResRoll)/2; |
534 | oldRoll = (oldRoll + HiResRoll)/2; |
489 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
535 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
- | 536 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
|
490 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
537 | |
- | 538 | //MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es |
|
491 | 539 | MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert |
|
492 | MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
540 | //MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; // MartinR : so war es |
493 | MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
541 | MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 2; // MartinR : geändert |
Line 494... | Line 542... | ||
494 | HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
542 | HiResNick += (d2Nick * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt |
495 | HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
543 | HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D * 8); // martinR: *8 hinzugefügt |
496 | } |
544 | } |
Line 572... | Line 620... | ||
572 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
620 | CHK_POTI_MM(Parameter_J17Timing,EE_Parameter.J17Timing,1,255); |
573 | CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3); |
621 | CHK_POTI(Parameter_Servo3,EE_Parameter.Servo3); |
574 | CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4); |
622 | CHK_POTI(Parameter_Servo4,EE_Parameter.Servo4); |
575 | CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5); |
623 | CHK_POTI(Parameter_Servo5,EE_Parameter.Servo5); |
576 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
624 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
577 | CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); |
625 | //CHK_POTI(Parameter_MaxHoehe,EE_Parameter.MaxHoehe); // MartinR: Zeile war doppelt |
578 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung); |
626 | CHK_POTI(Parameter_Hoehe_ACC_Wirkung,EE_Parameter.Hoehe_ACC_Wirkung); |
579 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z); |
627 | CHK_POTI(Parameter_Hoehe_GPS_Z,EE_Parameter.Hoehe_GPS_Z); |
580 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung); |
628 | CHK_POTI(Parameter_KompassWirkung,EE_Parameter.KompassWirkung); |
581 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I); |
629 | CHK_POTI(Parameter_Gyro_I,EE_Parameter.Gyro_I); |
582 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D); |
630 | CHK_POTI(Parameter_Gyro_D,EE_Parameter.Gyro_D); |
Line 599... | Line 647... | ||
599 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
647 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
600 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
648 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
601 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
649 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
602 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
650 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
603 | Ki = 10300 / (Parameter_I_Faktor + 1); |
651 | Ki = 10300 / (Parameter_I_Faktor + 1); |
- | 652 | ||
- | 653 | ||
- | 654 | if(Parameter_UserParam1 > 50) KiHH = 10300 / (Parameter_UserParam2 + 1); else KiHH = Ki; // MartinR : für HH über Schalter |
|
- | 655 | Parameter_NaviGpsModeControl = EE_Parameter.NaviGpsModeControl; //MartinR: Standard: EE_Parameter.NaviGpsModeControl wird übertragen |
|
- | 656 | if(!IntegralFaktor) Parameter_NaviGpsModeControl= 0; // MartinR: wenn HH dann GPS auf free- Mode |
|
- | 657 | // 0 = free; 100 = AID; 200 = coming home //neu |
|
- | 658 | ||
- | 659 | ||
604 | MAX_GAS = EE_Parameter.Gas_Max; |
660 | MAX_GAS = EE_Parameter.Gas_Max; |
605 | MIN_GAS = EE_Parameter.Gas_Min; |
661 | MIN_GAS = EE_Parameter.Gas_Min; |
Line 606... | Line 662... | ||
606 | 662 | ||
607 | tmp = EE_Parameter.OrientationModeControl; |
663 | tmp = EE_Parameter.OrientationModeControl; |
Line 631... | Line 687... | ||
631 | void MotorRegler(void) |
687 | void MotorRegler(void) |
632 | //############################################################################ |
688 | //############################################################################ |
633 | { |
689 | { |
634 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
690 | int pd_ergebnis_nick,pd_ergebnis_roll,tmp_int, tmp_int2; |
635 | int GierMischanteil,GasMischanteil; |
691 | int GierMischanteil,GasMischanteil; |
- | 692 | ||
- | 693 | static long SummeNickHH=0,SummeRollHH=0; // MartinR: hinzugefügt |
|
- | 694 | ||
636 | static long sollGier = 0,tmp_long,tmp_long2; |
695 | static long sollGier = 0,tmp_long,tmp_long2; |
637 | static long IntegralFehlerNick = 0; |
696 | static long IntegralFehlerNick = 0; |
638 | static long IntegralFehlerRoll = 0; |
697 | static long IntegralFehlerRoll = 0; |
639 | static unsigned int RcLostTimer; |
698 | static unsigned int RcLostTimer; |
640 | static unsigned char delay_neutral = 0; |
699 | static unsigned char delay_neutral = 0; |
Line 741... | Line 800... | ||
741 | SetNeutral(0); |
800 | SetNeutral(0); |
742 | calibration_done = 1; |
801 | calibration_done = 1; |
743 | ServoActive = 1; |
802 | ServoActive = 1; |
744 | DDRD |=0x80; // enable J7 -> Servo signal |
803 | DDRD |=0x80; // enable J7 -> Servo signal |
745 | Piep(GetActiveParamSet(),120); |
804 | Piep(GetActiveParamSet(),120); |
- | 805 | PPM_in[13] = Parameter_UserParam5 -110; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
|
- | 806 | PPM_in[14] = Parameter_UserParam6 -110; // MartinR: Initialisierungswerte für die seriellen Potis für Jeti+ |
|
746 | } |
807 | } |
747 | } |
808 | } |
748 | } |
809 | } |
749 | else |
810 | else |
750 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
811 | if(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]] < -75) // ACC Neutralwerte speichern |
Line 783... | Line 844... | ||
783 | { |
844 | { |
784 | modell_fliegt = 1; |
845 | modell_fliegt = 1; |
785 | MotorenEin = 1; |
846 | MotorenEin = 1; |
786 | sollGier = 0; |
847 | sollGier = 0; |
787 | Mess_Integral_Gier = 0; |
848 | Mess_Integral_Gier = 0; |
788 | Mess_Integral_Gier2 = 0; |
849 | //Mess_Integral_Gier2 = 0; //MartinR: Mess_Integral_Gier2 unbenutzt |
789 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
850 | Mess_IntegralNick = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccNick; |
790 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
851 | Mess_IntegralRoll = EE_Parameter.GyroAccFaktor * (long)Mittelwert_AccRoll; |
791 | Mess_IntegralNick2 = IntegralNick; |
852 | Mess_IntegralNick2 = IntegralNick; |
792 | Mess_IntegralRoll2 = IntegralRoll; |
853 | Mess_IntegralRoll2 = IntegralRoll; |
793 | SummeNick = 0; |
854 | SummeNick = 0; |
Line 827... | Line 888... | ||
827 | 888 | ||
828 | if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
889 | if(!NewPpmData-- || (FC_StatusFlags & FC_STATUS_EMERGENCY_LANDING)) |
829 | { |
890 | { |
830 | static int stick_nick,stick_roll; |
891 | static int stick_nick,stick_roll; |
- | 892 | ParameterZuordnung(); |
|
- | 893 | // MartinR: original: |
|
831 | ParameterZuordnung(); |
894 | /* |
832 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
895 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
833 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
896 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
834 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
897 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
- | 898 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
|
- | 899 | |
|
- | 900 | */ |
|
- | 901 | // MartinR: geändert Anfang |
|
- | 902 | if(Parameter_UserParam1 > 50) // MartinR: zweiter Stick_P Wert nur, wenn HH über Schalter aktiv ist |
|
- | 903 | { |
|
- | 904 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral) / 4; |
|
- | 905 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral) / 4 ; |
|
- | 906 | //stick_nick = (PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * Parameter_UserParam3 - stick_nick_neutral); |
|
- | 907 | //stick_roll = (PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * Parameter_UserParam3 - stick_roll_neutral); |
|
- | 908 | } |
|
- | 909 | ||
- | 910 | else |
|
- | 911 | { |
|
- | 912 | stick_nick = (stick_nick * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_P) / 4; |
|
- | 913 | stick_roll = (stick_roll * 3 + PPM_in[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_P) / 4; |
|
- | 914 | stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 915 | stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 916 | } |
|
- | 917 | ||
- | 918 | if(IntegralFaktor) |
|
- | 919 | { |
|
- | 920 | //stick_nick_neutral = stick_nick; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 921 | //stick_roll_neutral = stick_roll; // beim Umschalten auf HH wird derletzte Stickwert als Neutralposition verwendet, MartinR |
|
- | 922 | stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; |
|
- | 923 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
|
- | 924 | ||
- | 925 | //StickNick = stick_nick - (GPS_Nick + GPS_Nick2); // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 926 | //StickRoll = stick_roll - (GPS_Roll + GPS_Roll2); // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 927 | } |
|
- | 928 | /*else // wenn HH , MartinR |
|
- | 929 | { |
|
- | 930 | //stick_nick += PPM_diff[EE_Parameter.Kanalbelegung[K_NICK]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
|
- | 931 | //stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; // MartinR: eventuell vor if verschieben |
|
- | 932 | //StickNick = stick_nick; // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 933 | //StickRoll = stick_roll; // MartinR: GPS nur im ACC-Mode wirksam |
|
- | 934 | } |
|
- | 935 | */ |
|
Line 835... | Line 936... | ||
835 | stick_roll += PPM_diff[EE_Parameter.Kanalbelegung[K_ROLL]] * EE_Parameter.Stick_D; |
936 | // MartinR: geändert Ende |
836 | 937 | ||
837 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
938 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
838 | // CareFree und freie Wahl der vorderen Richtung |
939 | // CareFree und freie Wahl der vorderen Richtung |
Line 860... | Line 961... | ||
860 | StickNick -= (GPS_Nick + GPS_Nick2); |
961 | StickNick -= (GPS_Nick + GPS_Nick2); |
861 | StickRoll -= (GPS_Roll + GPS_Roll2); |
962 | StickRoll -= (GPS_Roll + GPS_Roll2); |
862 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
963 | StickGas = PPM_in[EE_Parameter.Kanalbelegung[K_GAS]] + 120; |
Line 863... | Line 964... | ||
863 | 964 | ||
864 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
965 | GyroFaktor = (Parameter_Gyro_P + 10.0); |
865 | IntegralFaktor = Parameter_Gyro_I; |
966 | // IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen |
866 | GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
967 | GyroFaktorGier = (Parameter_Gyro_Gier_P + 10.0); |
867 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
- | |
868 | 968 | IntegralFaktorGier = Parameter_Gyro_Gier_I; |
|
869 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
969 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
870 | //+ Analoge Steuerung per Seriell |
970 | //+ Analoge Steuerung per Seriell |
- | 971 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 972 | // MartinR: ToDo: eventuell die Kombination HH und Steuerung per Seriell nicht zulassen?? |
|
871 | //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
973 | |
872 | if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
974 | if(ExternControl.Config & 0x01 && Parameter_ExternalControl > 128) |
873 | { |
975 | { |
874 | StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
976 | StickNick += (int) ExternControl.Nick * (int) EE_Parameter.Stick_P; |
875 | StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
977 | StickRoll += (int) ExternControl.Roll * (int) EE_Parameter.Stick_P; |
876 | StickGier += ExternControl.Gier; |
978 | StickGier += ExternControl.Gier; |
877 | ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
979 | ExternHoehenValue = (int) ExternControl.Hight * (int)EE_Parameter.Hoehe_Verstaerkung; |
878 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
980 | if(ExternControl.Gas < StickGas) StickGas = ExternControl.Gas; |
879 | } |
981 | } |
Line 880... | Line 982... | ||
880 | if(StickGas < 0) StickGas = 0; |
982 | if(StickGas < 0) StickGas = 0; |
Line 881... | Line 983... | ||
881 | 983 | ||
882 | if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; |
984 | //if(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) IntegralFaktor = 0; // MartinR: so war es, verschoben um code zu sparen |
883 | 985 | ||
884 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
986 | if(abs(StickNick/STICK_GAIN) > MaxStickNick) |
Line 967... | Line 1069... | ||
967 | MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
1069 | MittelIntegralNick += IntegralNick; // Für die Mittelwertbildung aufsummieren |
968 | MittelIntegralRoll += IntegralRoll; |
1070 | MittelIntegralRoll += IntegralRoll; |
969 | MittelIntegralNick2 += IntegralNick2; |
1071 | MittelIntegralNick2 += IntegralNick2; |
970 | MittelIntegralRoll2 += IntegralRoll2; |
1072 | MittelIntegralRoll2 += IntegralRoll2; |
Line 971... | Line 1073... | ||
971 | 1073 | ||
- | 1074 | // if(Looping_Nick || Looping_Roll) // MartinR: so war es |
|
972 | if(Looping_Nick || Looping_Roll) |
1075 | if(Looping_Nick || Looping_Roll || (!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD))) // MartinR: erweitert |
973 | { |
1076 | { |
974 | IntegralAccNick = 0; |
1077 | IntegralAccNick = 0; |
975 | IntegralAccRoll = 0; |
1078 | IntegralAccRoll = 0; |
976 | MittelIntegralNick = 0; |
1079 | MittelIntegralNick = 0; |
Line 981... | Line 1084... | ||
981 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
1084 | Mess_IntegralRoll2 = Mess_IntegralRoll; |
982 | ZaehlMessungen = 0; |
1085 | ZaehlMessungen = 0; |
983 | LageKorrekturNick = 0; |
1086 | LageKorrekturNick = 0; |
984 | LageKorrekturRoll = 0; |
1087 | LageKorrekturRoll = 0; |
985 | } |
1088 | } |
- | 1089 | ||
- | 1090 | if((!IntegralFaktor & (Parameter_UserParam1 < 50) & !(EE_Parameter.GlobalConfig & CFG_HEADING_HOLD)) ) // MartinR: |
|
- | 1091 | // nur im Moment des Umschaltens von HH auf ACC erfolgt ein Reset der Integrale, nicht aber bei normalem HH |
|
- | 1092 | // um einen im HH-Mode eventuell schwindelig geflogenen ACC_Mode zu resetten! |
|
- | 1093 | // bis zur Umschaltung werden die Integrale für den Kameraausgleich verwendet |
|
- | 1094 | { |
|
- | 1095 | ||
- | 1096 | IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1097 | IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1098 | Mess_IntegralNick = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1099 | Mess_IntegralRoll = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1100 | //Mess_Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1101 | //Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1102 | //Mess_Integral_Gier2 = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1103 | } |
|
- | 1104 | ||
- | 1105 | if((EE_Parameter.GlobalConfig & CFG_HEADING_HOLD) || (Parameter_UserParam1 > 50)) IntegralFaktor = 0; // MartinR geändert und verschoben |
|
- | 1106 | else IntegralFaktor = Parameter_Gyro_I; // MartinR: verschoben um Code zu sparen |
|
Line 986... | Line 1107... | ||
986 | 1107 | ||
987 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1108 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 1109 | if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) // MartinR: so war es, wegen Kameraausgleich wieder aktiviert |
|
- | 1110 | //if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin) && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt |
|
988 | if(!Looping_Nick && !Looping_Roll && (Aktuell_az > 512 || MotorenEin)) |
1111 | |
989 | { |
1112 | { |
990 | long tmp_long, tmp_long2; |
1113 | long tmp_long, tmp_long2; |
991 | if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/) |
1114 | if(FromNaviCtrl_Value.Kalman_K != -1 /*&& !TrichterFlug*/) |
992 | { |
1115 | { |
Line 1040... | Line 1163... | ||
1040 | if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
1163 | if(ZaehlMessungen >= ABGLEICH_ANZAHL) |
1041 | { |
1164 | { |
1042 | static int cnt = 0; |
1165 | static int cnt = 0; |
1043 | static char last_n_p,last_n_n,last_r_p,last_r_n; |
1166 | static char last_n_p,last_n_n,last_r_p,last_r_n; |
1044 | static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; |
1167 | static long MittelIntegralNick_Alt,MittelIntegralRoll_Alt; |
1045 | if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) |
1168 | //if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp) // MartinR: so war es |
- | 1169 | if(!Looping_Nick && !Looping_Roll && !TrichterFlug && EE_Parameter.Driftkomp && IntegralFaktor) // MartinR: "&& IntegralFaktor" hinzugefügt |
|
- | 1170 | ||
1046 | { |
1171 | { |
1047 | MittelIntegralNick /= ABGLEICH_ANZAHL; |
1172 | MittelIntegralNick /= ABGLEICH_ANZAHL; |
1048 | MittelIntegralRoll /= ABGLEICH_ANZAHL; |
1173 | MittelIntegralRoll /= ABGLEICH_ANZAHL; |
1049 | IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
1174 | IntegralAccNick = (EE_Parameter.GyroAccFaktor * IntegralAccNick) / ABGLEICH_ANZAHL; |
1050 | IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
1175 | IntegralAccRoll = (EE_Parameter.GyroAccFaktor * IntegralAccRoll) / ABGLEICH_ANZAHL; |
Line 1206... | Line 1331... | ||
1206 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1331 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 1207... | Line 1332... | ||
1207 | 1332 | ||
1208 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1333 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1209 | // Kompass |
1334 | // Kompass |
- | 1335 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
1210 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1336 | if((KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) && !(Parameter_UserParam1 > 50)) // MartinR: bei HH über Schalter wird der Kompass abgeschaltet |
1211 | if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) |
1337 | //if(KompassValue && (EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV)) // MartinR: so war es |
1212 | { |
1338 | { |
1213 | int w,v,r,fehler,korrektur; |
1339 | int w,v,r,fehler,korrektur; |
1214 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1340 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1215 | v = abs(IntegralRoll /512); |
1341 | v = abs(IntegralRoll /512); |
Line 1259... | Line 1385... | ||
1259 | 1385 | ||
1260 | #define TRIM_MAX 200 |
1386 | #define TRIM_MAX 200 |
1261 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
1387 | if(TrimNick > TRIM_MAX) TrimNick = TRIM_MAX; else if(TrimNick <-TRIM_MAX) TrimNick =-TRIM_MAX; |
Line -... | Line 1388... | ||
- | 1388 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
|
- | 1389 | ||
- | 1390 | //MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN);// MartinR so war es |
|
- | 1391 | //MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN);// MartinR so war es |
|
- | 1392 | ||
- | 1393 | if(!IntegralFaktor) // MartinR : HH-Mode hinzugefügt |
|
- | 1394 | { |
|
- | 1395 | MesswertNick = (long) ((long)MesswertNick * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt |
|
- | 1396 | MesswertRoll = (long) ((long)MesswertRoll * GyroFaktor) / (256L / STICK_GAIN) ; // MartinR : hinzugefügt |
|
- | 1397 | //MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN); |
|
- | 1398 | //Mess_Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 //neu: nur Kompass wird bei HH deaktiviert |
|
- | 1399 | //Integral_Gier = 0; // MartinR: im HH-Modus alle unbenutzten Integratoren = 0 |
|
- | 1400 | } |
|
1262 | if(TrimRoll > TRIM_MAX) TrimRoll = TRIM_MAX; else if(TrimRoll <-TRIM_MAX) TrimRoll =-TRIM_MAX; |
1401 | else // MartinR: ACC-Mode so war es |
1263 | 1402 | { |
|
- | 1403 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
|
- | 1404 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
|
- | 1405 | //MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
|
1264 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
1406 | } |
Line 1265... | Line 1407... | ||
1265 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
1407 | |
1266 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
1408 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
1267 | 1409 | ||
Line 1352... | Line 1494... | ||
1352 | } |
1494 | } |
Line 1353... | Line 1495... | ||
1353 | 1495 | ||
1354 | // if height control is activated by an rc channel |
1496 | // if height control is activated by an rc channel |
1355 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1497 | if(EE_Parameter.GlobalConfig & CFG_HOEHEN_SCHALTER) // Regler wird über Schalter gesteuert |
1356 | { // check if parameter is less than activation threshold |
1498 | { // check if parameter is less than activation threshold |
- | 1499 | // if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position // MartinR :so war es |
|
1357 | if(Parameter_MaxHoehe < 50) // for 3 or 2-state switch height control is disabled in lowest position |
1500 | if(Parameter_MaxHoehe < 50 || (Parameter_UserParam1 > 140) ) // MartinR: Schalter aus oder HH ohne Höhenregler über UsererParam1 an |
1358 | { //height control not active |
1501 | { //height control not active |
1359 | if(!delay--) |
1502 | if(!delay--) |
1360 | { |
1503 | { |
1361 | HoehenReglerAktiv = 0; // disable height control |
1504 | HoehenReglerAktiv = 0; // disable height control |
Line 1370... | Line 1513... | ||
1370 | } |
1513 | } |
1371 | } |
1514 | } |
1372 | else // no switchable height control |
1515 | else // no switchable height control |
1373 | { |
1516 | { |
1374 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
1517 | SollHoehe = ((int16_t) ExternHoehenValue + (int16_t) Parameter_MaxHoehe) * (int)EE_Parameter.Hoehe_Verstaerkung; |
- | 1518 | // HoehenReglerAktiv = 1; // MartinR : so war es |
|
- | 1519 | // MartinR : geändert Anfang |
|
- | 1520 | if(Parameter_UserParam1 > 140) // HH über Schalter: HH an + Höhenregler abgeschaltet, Nachführen von Parametern |
|
- | 1521 | { |
|
- | 1522 | HoehenReglerAktiv = 0; |
|
- | 1523 | } |
|
- | 1524 | else // Höhenregler mit Sollhöhe über Poti aktiv |
|
- | 1525 | { |
|
1375 | HoehenReglerAktiv = 1; |
1526 | HoehenReglerAktiv = 1; |
- | 1527 | } |
|
- | 1528 | // MartinR : geändert Ende |
|
1376 | } |
1529 | } |
Line 1377... | Line 1530... | ||
1377 | 1530 | ||
1378 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1531 | // calculate cos of nick and roll angle used for projection of the vertical hoover gas |
1379 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
1532 | tmp_int = (int)(IntegralNick/GIER_GRAD_FAKTOR); // nick angle in deg |
Line 1432... | Line 1585... | ||
1432 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1585 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) |
1433 | { |
1586 | { |
1434 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1587 | FC_StatusFlags &= ~(FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN); |
1435 | HeightTrimming = 0; |
1588 | HeightTrimming = 0; |
1436 | SollHoehe = HoehenWert; // update setpoint to current height |
1589 | SollHoehe = HoehenWert; // update setpoint to current height |
- | 1590 | //MartinR: Sollhöhe sollte sich hier nicht mehr ändern! |
|
1437 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1591 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 500; |
1438 | if(!StartTrigger && HoehenWert > 50) |
1592 | if(!StartTrigger && HoehenWert > 50) |
1439 | { |
1593 | { |
1440 | StartTrigger = 1; |
1594 | StartTrigger = 1; |
1441 | } |
1595 | } |
Line 1445... | Line 1599... | ||
1445 | // Trim height set point |
1599 | // Trim height set point |
1446 | if(abs(HeightTrimming) > 512) |
1600 | if(abs(HeightTrimming) > 512) |
1447 | { |
1601 | { |
1448 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1602 | SollHoehe += (HeightTrimming * EE_Parameter.Hoehe_Verstaerkung)/(5 * 512 / 2); // move setpoint |
1449 | HeightTrimming = 0; |
1603 | HeightTrimming = 0; |
1450 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied |
1604 | //LIMIT_MIN_MAX(SollHoehe, (HoehenWert-1024), (HoehenWert+1024)); // max. 10m Unterschied // MartinR: so war es |
- | 1605 | LIMIT_MIN_MAX(SollHoehe, (HoehenWert-256), (HoehenWert+256)); // MartinR: geändert |
|
- | 1606 | // MartinR: limits eventuell verändern |
|
1451 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1607 | if(EE_Parameter.ExtraConfig & CFG2_VARIO_BEEP) beeptime = 100; |
1452 | //update hoover gas stick value when setpoint is shifted |
1608 | //update hoover gas stick value when setpoint is shifted |
1453 | if(!EE_Parameter.Hoehe_StickNeutralPoint) |
1609 | if(!EE_Parameter.Hoehe_StickNeutralPoint) |
1454 | { |
1610 | { |
1455 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
1611 | StickGasHover = HoverGas/STICK_GAIN; //rescale back to stick value |
Line 1457... | Line 1613... | ||
1457 | if(StickGasHover < 70) StickGasHover = 70; |
1613 | if(StickGasHover < 70) StickGasHover = 70; |
1458 | else if(StickGasHover > 150) StickGasHover = 150; |
1614 | else if(StickGasHover > 150) StickGasHover = 150; |
1459 | } |
1615 | } |
1460 | } |
1616 | } |
1461 | if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active |
1617 | if(BaroExpandActive) SollHoehe = HoehenWert; // update setpoint to current altitude if Expanding is active |
1462 | } //if FCFlags & MKFCFLAG_FLY |
1618 | } //if FC_StatusFlags & MKFCFLAG_FLY |
1463 | else |
1619 | else |
1464 | { |
1620 | { |
1465 | SollHoehe = HoehenWert - 400; |
1621 | SollHoehe = HoehenWert - 400; |
1466 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
1622 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
1467 | else StickGasHover = 120; |
1623 | else StickGasHover = 120; |
Line 1478... | Line 1634... | ||
1478 | HeightDeviation = 0; |
1634 | HeightDeviation = 0; |
1479 | } // EOF // baro range expanding active |
1635 | } // EOF // baro range expanding active |
1480 | else // valid data from air pressure sensor |
1636 | else // valid data from air pressure sensor |
1481 | { |
1637 | { |
1482 | // ------------------------- P-Part ---------------------------- |
1638 | // ------------------------- P-Part ---------------------------- |
- | 1639 | //DebugOut.Analog[16] = SollHoehe; // MartinR: test |
|
1483 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1640 | tmp_long = (HoehenWert - SollHoehe); // positive when too high |
1484 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1641 | LIMIT_MIN_MAX(tmp_long, -32767L, 32767L); // avoid overflov when casting to int16_t |
1485 | HeightDeviation = (int)(tmp_long); // positive when too high |
1642 | HeightDeviation = (int)(tmp_long); // positive when too high |
1486 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1643 | tmp_long = (tmp_long * (long)Parameter_Hoehe_P) / 32L; // p-part |
1487 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense |
1644 | LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 256 * STICK_GAIN); // more than the full range makes no sense // MartinR: so war es |
- | 1645 | // MartinR: weshalb unsymmetrisch? |
|
- | 1646 | //LIMIT_MIN_MAX(tmp_long, -127 * STICK_GAIN, 127 * STICK_GAIN); // more than the full range makes no sense // MartinR: geändert |
|
1488 | GasReduction = tmp_long; |
1647 | GasReduction = tmp_long; |
1489 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1648 | // ------------------------- D-Part 1: Vario Meter ---------------------------- |
1490 | tmp_int = VarioMeter / 8; |
1649 | tmp_int = VarioMeter / 8; |
1491 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1650 | LIMIT_MIN_MAX(tmp_int, -127, 128); |
1492 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1651 | tmp_int = (tmp_int * (long)Parameter_Luftdruck_D) / 4L; // scale to d-gain parameter |
1493 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
1652 | LIMIT_MIN_MAX(tmp_int,-64 * STICK_GAIN, 64 * STICK_GAIN); |
- | 1653 | /* // MartinR: so war es Anfang |
|
1494 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
1654 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint |
1495 | else |
1655 | else |
1496 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
1656 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) tmp_int /= 8; // reduce d-part in "Deckel" mode |
- | 1657 | */ // MartinR: so war es Ende |
|
- | 1658 | tmp_int /= 4; // MartinR: geändert: keine veränderung des d-part im "Deckel" mode |
|
- | 1659 | if(FC_StatusFlags & (FC_STATUS_VARIO_TRIM_UP|FC_STATUS_VARIO_TRIM_DOWN)) tmp_int /= 4; // reduce d-part while trimming setpoint // MartinR: geändert |
|
1497 | GasReduction += tmp_int; |
1660 | GasReduction += tmp_int; |
1498 | } // EOF no baro range expanding |
1661 | } // EOF no baro range expanding |
1499 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1662 | // ------------------------ D-Part 2: ACC-Z Integral ------------------------ |
1500 | if(Parameter_Hoehe_ACC_Wirkung) |
1663 | if(Parameter_Hoehe_ACC_Wirkung) |
1501 | { |
1664 | { |
1502 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1665 | tmp_long = ((Mess_Integral_Hoch / 128L) * (int32_t) Parameter_Hoehe_ACC_Wirkung) / (128L / STICK_GAIN); |
1503 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1666 | //LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 64 * STICK_GAIN); // MartinR: so war es |
- | 1667 | // MartinR: weshalb unsymmetrisch? |
|
- | 1668 | LIMIT_MIN_MAX(tmp_long, -32 * STICK_GAIN, 32 * STICK_GAIN); // MartinR: geändert |
|
1504 | GasReduction += tmp_long; |
1669 | GasReduction += tmp_long; |
1505 | } |
1670 | } |
1506 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1671 | // ------------------------ D-Part 3: GpsZ ---------------------------------- |
1507 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1672 | tmp_int = (Parameter_Hoehe_GPS_Z * (int)FromNaviCtrl_Value.GpsZ)/128L; |
1508 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); |
1673 | //LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 64 * STICK_GAIN); // MartinR: so war es |
- | 1674 | // MartinR: weshalb unsymmetrisch? |
|
- | 1675 | LIMIT_MIN_MAX(tmp_int, -32 * STICK_GAIN, 32 * STICK_GAIN); // MartinR: geändert |
|
1509 | GasReduction += tmp_int; |
1676 | GasReduction += tmp_int; |
1510 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
1677 | GasReduction = (long)((long)GasReduction * HoverGas) / 512; // scale to the gas value |
- | 1678 | //DebugOut.Analog[17] = GasReduction; // MartinR: test |
|
1511 | // ------------------------ ---------------------------------- |
1679 | // ------------------------ ---------------------------------- |
1512 | HCGas -= GasReduction; |
1680 | HCGas -= GasReduction; |
1513 | // limit deviation from hoover point within the target region |
1681 | // limit deviation from hoover point within the target region |
1514 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1682 | if(!HeightTrimming && HoverGas > 0) // height setpoint is not changed and hoover gas not zero |
1515 | { |
1683 | { |
Line 1549... | Line 1717... | ||
1549 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
1717 | if(EE_Parameter.ExtraConfig & CFG2_HEIGHT_LIMIT) |
1550 | { // old version |
1718 | { // old version |
1551 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1719 | LIMIT_MAX(FilterHCGas, GasMischanteil); // nicht mehr als Gas |
1552 | GasMischanteil = FilterHCGas; |
1720 | GasMischanteil = FilterHCGas; |
1553 | } |
1721 | } |
1554 | else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode |
1722 | //else GasMischanteil = FilterHCGas + (GasMischanteil - HoverGas) / 4; // only in Vario-Mode // MartinR: so war es |
- | 1723 | else GasMischanteil = FilterHCGas ; // MartinR: geändert, um Überschwinger bei Höhenänderung zu verringern |
|
1555 | } |
1724 | } |
1556 | }// EOF height control active |
1725 | }// EOF height control active |
1557 | else // HC not active |
1726 | else // HC not active |
1558 | { |
1727 | { |
1559 | //update hoover gas stick value when HC is not active |
1728 | //update hoover gas stick value when HC is not active |
Line 1655... | Line 1824... | ||
1655 | if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2); |
1824 | if(GierMischanteil < -(MIN_GIERGAS / 2)) GierMischanteil = -(MIN_GIERGAS / 2); |
1656 | } |
1825 | } |
1657 | tmp_int = MAX_GAS*STICK_GAIN; |
1826 | tmp_int = MAX_GAS*STICK_GAIN; |
1658 | if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
1827 | if(GierMischanteil > ((tmp_int - GasMischanteil))) GierMischanteil = ((tmp_int - GasMischanteil)); |
1659 | if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
1828 | if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
- | 1829 | ||
- | 1830 | ||
- | 1831 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1832 | // Nick / Roll-Achse // MartinR: um Code zu sparen wurde Nick und Roll zusammengefasst |
|
- | 1833 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1834 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
|
- | 1835 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
|
- | 1836 | ||
- | 1837 | // PI-Regler für Nick und Roll |
|
- | 1838 | if(EE_Parameter.Gyro_Stability <= 8) |
|
- | 1839 | { |
|
- | 1840 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen |
|
- | 1841 | pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8; |
|
- | 1842 | } |
|
- | 1843 | else |
|
- | 1844 | { |
|
- | 1845 | pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern |
|
- | 1846 | pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4; // Überlauf verhindern |
|
- | 1847 | } |
|
- | 1848 | ||
- | 1849 | if(IntegralFaktor) // MartinR : ACC-Mode |
|
- | 1850 | { |
|
- | 1851 | SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
|
- | 1852 | if(SummeNick > (STICK_GAIN * 8000L)) SummeNick = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 1853 | if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 1854 | pd_ergebnis_nick += (SummeNick / Ki); // PI-Regler für Nick |
|
- | 1855 | SummeNickHH = 0 ; |
|
- | 1856 | ||
- | 1857 | SummeRoll += IntegralRollMalFaktor - StickRoll; |
|
- | 1858 | if(SummeRoll > (STICK_GAIN * 8000L)) SummeRoll = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 1859 | if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 1860 | pd_ergebnis_roll += (SummeRoll / Ki); // PI-Regler für Roll |
|
- | 1861 | SummeRollHH = 0; |
|
- | 1862 | ||
- | 1863 | } |
|
- | 1864 | else // MartinR : HH-Mode |
|
- | 1865 | { |
|
- | 1866 | SummeNickHH += DiffNick; // I-Anteil bei HH |
|
- | 1867 | if(SummeNickHH > (STICK_GAIN * 8000L)) SummeNickHH = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 1868 | if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 1869 | pd_ergebnis_nick += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH |
|
- | 1870 | SummeNick = 0; |
|
- | 1871 | ||
- | 1872 | SummeRollHH += DiffRoll; // I-Anteil bei HH |
|
- | 1873 | if(SummeRollHH > (STICK_GAIN * 8000L)) SummeRollHH = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 1874 | if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 1875 | pd_ergebnis_roll += SummeRollHH / KiHH; // MartinR: PI-Regler für Roll bei HH |
|
- | 1876 | SummeRoll = 0; |
|
- | 1877 | } |
|
- | 1878 | // MartinR : geändert Ende |
|
- | 1879 | ||
- | 1880 | ||
- | 1881 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
|
- | 1882 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
|
- | 1883 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
|
Line -... | Line 1884... | ||
- | 1884 | ||
- | 1885 | //tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
|
- | 1886 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
|
- | 1887 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
|
- | 1888 | ||
- | 1889 | ||
- | 1890 | // MartinR: alt |
|
1660 | 1891 | /* |
|
1661 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1892 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1662 | // Nick-Achse |
1893 | // Nick-Achse |
1663 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1894 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
- | 1895 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
|
- | 1896 | // MartinR : so war es Anfang |
|
1664 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1897 | /+ |
1665 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1898 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1666 | else SummeNick += DiffNick; // I-Anteil bei HH |
1899 | else SummeNick += DiffNick; // I-Anteil bei HH |
1667 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1900 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
- | 1901 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
|
- | 1902 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
|
1668 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1903 | // Motor Vorn |
- | 1904 | |
|
- | 1905 | +/ |
|
- | 1906 | // MartinR : so war es Ende |
|
- | 1907 | |
|
1669 | 1908 | // MartinR : geändert Anfang |
|
- | 1909 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 ; // Zwischenergebnis um Code zu sparen |
|
- | 1910 | if(IntegralFaktor) // MartinR : ACC-Mode |
|
- | 1911 | { |
|
- | 1912 | SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
|
1670 | if(EE_Parameter.Gyro_Stability <= 8) pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8; // PI-Regler für Nick |
1913 | if(SummeNick > (STICK_GAIN * 8000L)) SummeNick = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
1671 | else pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern |
1914 | if(SummeNick < -(8000L * STICK_GAIN)) SummeNick = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
- | 1915 | //pd_ergebnis_nick = DiffNick + (SummeNick / Ki); |
|
- | 1916 | pd_ergebnis_nick += (SummeNick / Ki); // PI-Regler für Nick |
|
- | 1917 | SummeNickHH = 0 ; |
|
- | 1918 | } |
|
- | 1919 | else // MartinR : HH-Mode |
|
- | 1920 | { |
|
- | 1921 | SummeNickHH += DiffNick; // I-Anteil bei HH |
|
- | 1922 | if(SummeNickHH > (STICK_GAIN * 8000L)) SummeNickHH = (STICK_GAIN * 8000L); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 1923 | if(SummeNickHH < -(8000L * STICK_GAIN)) SummeNickHH = -(8000L * STICK_GAIN); // MartinR : von 16000 auf 8000, da überlauf |
|
- | 1924 | pd_ergebnis_nick += SummeNickHH / KiHH; // MartinR: PI-Regler für Nick bei HH |
|
- | 1925 | SummeNick = 0; |
|
- | 1926 | } |
|
- | 1927 | // MartinR : geändert Ende |
|
1672 | pd_ergebnis_nick += SummeNick / Ki; |
1928 | |
1673 | 1929 | |
|
1674 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1930 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1675 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1931 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
Line 1676... | Line 1932... | ||
1676 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
1932 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
1677 | 1933 | ||
1678 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1934 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1679 | // Roll-Achse |
1935 | // Roll-Achse |
- | 1936 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
- | 1937 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
|
1680 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1938 | // MartinR : so war es Anfang |
1681 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1939 | /+ |
1682 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1940 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1683 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1941 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
- | 1942 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
|
- | 1943 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
|
- | 1944 | pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki; // PI-Regler für Roll |
|
1684 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1945 | +/ |
- | 1946 | // MartinR : so war es Ende |
|
1685 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1947 | |
- | 1948 | // MartinR : geändert Anfang |
|
- | 1949 | pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8; |
|
- | 1950 | if(IntegralFaktor) // MartinR : ACC-Mode |
|
- | 1951 | { |
|
- | 1952 | SummeRoll += IntegralRollMalFaktor - StickRoll; |
|
- | 1953 | if(SummeRoll > (STICK_GAIN * 8000L)) SummeRoll = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
|
1686 | 1954 | if(SummeRoll < -(8000L * STICK_GAIN)) SummeRoll = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
|
1687 | if(EE_Parameter.Gyro_Stability <= 8) pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8; // PI-Regler für Roll |
1955 | //tmp_int = SummeRoll / Ki; |
- | 1956 | //pd_ergebnis_roll = DiffRoll + tmp_int; // MartinR: PI-Regler im ACC-Mode |
|
- | 1957 | pd_ergebnis_roll += SummeRoll / Ki; // PI-Regler für Roll |
|
- | 1958 | //SummeRollHH = (IntegralRollMalFaktor + tmp_int - stick_roll_neutral + (TrimRoll * STICK_GAIN / 2)) * KiHH;// MartinR: Startwert von SummeRollHH bei Umschaltung auf HH |
|
- | 1959 | // MartinR: Hintergrund: pd_ergebnis_xx soll sich beim Umschalten nicht ändern! |
|
- | 1960 | SummeRollHH = 0; |
|
- | 1961 | } |
|
- | 1962 | else // MartinR : HH-Mode |
|
- | 1963 | { |
|
- | 1964 | SummeRollHH += DiffRoll; // I-Anteil bei HH |
|
- | 1965 | if(SummeRollHH > (STICK_GAIN * 8000L)) SummeRollHH = (STICK_GAIN * 8000L);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 1966 | if(SummeRollHH < -(8000L * STICK_GAIN)) SummeRollHH = -(8000L * STICK_GAIN);// MartinR : von 16000 auf 8000, da überlauf |
|
- | 1967 | pd_ergebnis_roll += SummeRollHH / KiHH; // MartinR: PI-Regler für Roll bei HH |
|
- | 1968 | SummeRoll = 0; |
|
Line 1688... | Line 1969... | ||
1688 | else pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4; // Überlauf verhindern |
1969 | } |
1689 | pd_ergebnis_roll += SummeRoll / Ki; |
1970 | // MartinR : geändert Ende |
1690 | 1971 | |
|
- | 1972 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
|
- | 1973 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
|
Line 1691... | Line 1974... | ||
1691 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1974 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1692 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1975 | |
1693 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1976 | */ |
1694 | 1977 | ||
Line 1714... | Line 1997... | ||
1714 | if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil; |
1997 | if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil; |
1715 | else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil; |
1998 | else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil; |
1716 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1999 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
Line 1717... | Line 2000... | ||
1717 | 2000 | ||
1718 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
2001 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
- | 2002 | //else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing // MartinR: so war es |
|
Line 1719... | Line 2003... | ||
1719 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
2003 | //else tmp_int = tmp_int; // MartinR: Entsprechend Vorschlag von MartinW geändert |
1720 | 2004 | ||
1721 | LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4); |
2005 | LIMIT_MIN_MAX(tmp_int,(int) MIN_GAS * 4,(int) MAX_GAS * 4); |
1722 | Motor[i].SetPoint = tmp_int / 4; |
2006 | Motor[i].SetPoint = tmp_int / 4; |