Rev 1787 | Rev 1834 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1787 | Rev 1791 | ||
---|---|---|---|
Line 481... | Line 481... | ||
481 | { |
481 | { |
482 | d2Nick = HiResNick - oldNick; |
482 | d2Nick = HiResNick - oldNick; |
483 | oldNick = (oldNick + HiResNick)/2; |
483 | oldNick = (oldNick + HiResNick)/2; |
484 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
484 | if(d2Nick > D_LIMIT) d2Nick = D_LIMIT; |
485 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
485 | else if(d2Nick < -D_LIMIT) d2Nick = -D_LIMIT; |
486 | MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
- | |
- | 486 | ||
487 | d2Roll = HiResRoll - oldRoll; |
487 | d2Roll = HiResRoll - oldRoll; |
488 | oldRoll = (oldRoll + HiResRoll)/2; |
488 | oldRoll = (oldRoll + HiResRoll)/2; |
489 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
489 | if(d2Roll > D_LIMIT) d2Roll = D_LIMIT; |
490 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
490 | else if(d2Roll < -D_LIMIT) d2Roll = -D_LIMIT; |
- | 491 | ||
- | 492 | MesswertNick += (d2Nick * (signed int) Parameter_Gyro_D) / 16; |
|
491 | MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
493 | MesswertRoll += (d2Roll * (signed int) Parameter_Gyro_D) / 16; |
492 | HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
494 | HiResNick += (d2Nick * (signed int) Parameter_Gyro_D); |
493 | HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
495 | HiResRoll += (d2Roll * (signed int) Parameter_Gyro_D); |
494 | } |
496 | } |
Line 1662... | Line 1664... | ||
1662 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1664 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1663 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1665 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1664 | else SummeNick += DiffNick; // I-Anteil bei HH |
1666 | else SummeNick += DiffNick; // I-Anteil bei HH |
1665 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1667 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1666 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1668 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
- | 1669 | ||
1667 | pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8 + SummeNick / Ki; // PI-Regler für Nick |
1670 | if(EE_Parameter.Gyro_Stability <= 8) pd_ergebnis_nick = (EE_Parameter.Gyro_Stability * DiffNick) / 8; // PI-Regler für Nick |
- | 1671 | else pd_ergebnis_nick = ((EE_Parameter.Gyro_Stability / 2) * DiffNick) / 4; // Überlauf verhindern |
|
1668 | // Motor Vorn |
1672 | pd_ergebnis_nick += SummeNick / Ki; |
- | 1673 | ||
1669 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1674 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1670 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1675 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1671 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
1676 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
Line 1672... | Line 1677... | ||
1672 | 1677 | ||
Line 1676... | Line 1681... | ||
1676 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1681 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1677 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1682 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1678 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1683 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1679 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1684 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1680 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1685 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
- | 1686 | ||
1681 | pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8 + SummeRoll / Ki; // PI-Regler für Roll |
1687 | if(EE_Parameter.Gyro_Stability <= 8) pd_ergebnis_roll = (EE_Parameter.Gyro_Stability * DiffRoll) / 8; // PI-Regler für Roll |
- | 1688 | else pd_ergebnis_roll = ((EE_Parameter.Gyro_Stability / 2) * DiffRoll) / 4; // Überlauf verhindern |
|
- | 1689 | pd_ergebnis_roll += SummeRoll / Ki; |
|
- | 1690 | ||
1682 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1691 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1683 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1692 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1684 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1693 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
Line 1685... | Line 1694... | ||
1685 | 1694 |