Rev 1787 | Rev 1834 | Go to most recent revision | Show entire file | Ignore 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 | ||