Rev 1675 | Rev 1677 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1675 | Rev 1676 | ||
|---|---|---|---|
| Line 97... | Line 97... | ||
| 97 | char MotorenEin = 0; |
97 | char MotorenEin = 0; |
| 98 | long HoehenWert = 0; |
98 | long HoehenWert = 0; |
| 99 | long SollHoehe = 0; |
99 | long SollHoehe = 0; |
| 100 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
100 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
| 101 | //float Ki = FAKTOR_I; |
101 | //float Ki = FAKTOR_I; |
| 102 | int Ki = (10300/2) / 33; |
102 | int Ki = 10300 / 33; |
| 103 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
103 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
| 104 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
104 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
| Line 105... | Line 105... | ||
| 105 | 105 | ||
| 106 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
106 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
| Line 585... | Line 585... | ||
| 585 | CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2); |
585 | CHK_POTI(Parameter_AchsKopplung2,EE_Parameter.AchsKopplung2); |
| 586 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
586 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
| 587 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
587 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
| 588 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
588 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
| 589 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
589 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
| 590 | Ki = (10300 / 2) / (Parameter_I_Faktor + 1); |
590 | Ki = 10300 / (Parameter_I_Faktor + 1); |
| 591 | MAX_GAS = EE_Parameter.Gas_Max; |
591 | MAX_GAS = EE_Parameter.Gas_Max; |
| 592 | MIN_GAS = EE_Parameter.Gas_Min; |
592 | MIN_GAS = EE_Parameter.Gas_Min; |
| Line 593... | Line 593... | ||
| 593 | 593 | ||
| 594 | tmp = EE_Parameter.OrientationModeControl; |
594 | tmp = EE_Parameter.OrientationModeControl; |
| Line 1624... | Line 1624... | ||
| 1624 | else SummeNick += DiffNick; // I-Anteil bei HH |
1624 | else SummeNick += DiffNick; // I-Anteil bei HH |
| 1625 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1625 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
| 1626 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1626 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
| 1627 | pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
1627 | pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
| 1628 | // Motor Vorn |
1628 | // Motor Vorn |
| 1629 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / (64/2); |
1629 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
| 1630 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1630 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
| 1631 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
1631 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
| Line 1632... | Line 1632... | ||
| 1632 | 1632 | ||
| 1633 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1633 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 1637... | Line 1637... | ||
| 1637 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1637 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
| 1638 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1638 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
| 1639 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1639 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
| 1640 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1640 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
| 1641 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
1641 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
| 1642 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / (64/2); |
1642 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
| 1643 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1643 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
| 1644 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1644 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
| Line 1645... | Line 1645... | ||
| 1645 | 1645 | ||
| 1646 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1646 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 1650... | Line 1650... | ||
| 1650 | { |
1650 | { |
| 1651 | signed int tmp_int; |
1651 | signed int tmp_int; |
| 1652 | if(Mixer.Motor[i][0] > 0) |
1652 | if(Mixer.Motor[i][0] > 0) |
| 1653 | { |
1653 | { |
| 1654 | // Gas |
1654 | // Gas |
| 1655 | if(Mixer.Motor[i][0] == 64) tmp_int = GasMischanteil * 2; else tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 32L; |
1655 | if(Mixer.Motor[i][0] == 64) tmp_int = GasMischanteil; else tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
| 1656 | // Nick |
1656 | // Nick |
| 1657 | if(Mixer.Motor[i][1] == 64) tmp_int += pd_ergebnis_nick; |
1657 | if(Mixer.Motor[i][1] == 64) tmp_int += pd_ergebnis_nick; |
| 1658 | else if(Mixer.Motor[i][1] == -64) tmp_int -= pd_ergebnis_nick; |
1658 | else if(Mixer.Motor[i][1] == -64) tmp_int -= pd_ergebnis_nick; |
| 1659 | else tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
1659 | else tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
| 1660 | // Roll |
1660 | // Roll |
| 1661 | if(Mixer.Motor[i][2] == 64) tmp_int += pd_ergebnis_roll; |
1661 | if(Mixer.Motor[i][2] == 64) tmp_int += pd_ergebnis_roll; |
| 1662 | else if(Mixer.Motor[i][2] == -64) tmp_int -= pd_ergebnis_roll; |
1662 | else if(Mixer.Motor[i][2] == -64) tmp_int -= pd_ergebnis_roll; |
| 1663 | else tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
1663 | else tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
| 1664 | // Gier |
1664 | // Gier |
| 1665 | if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil * 2; |
1665 | if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil; |
| 1666 | else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil * 2; |
1666 | else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil; |
| 1667 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 32L; |
1667 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
| Line 1668... | Line 1668... | ||
| 1668 | 1668 | ||
| 1669 | if(Parameter_UserParam5 > 50) |
1669 | if(Parameter_UserParam5 > 50) |
| 1670 | { |
1670 | { |
| 1671 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
1671 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
| 1672 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
1672 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
| Line 1673... | Line -... | ||
| 1673 | } |
- | |
| 1674 | - | ||
| 1675 | /* |
- | |
| 1676 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
- | |
| 1677 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
- | |
| 1678 | if(Poti2 > 50) |
- | |
| 1679 | { |
- | |
| 1680 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
- | |
| 1681 | else if(Poti2 > 150) tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
- | |
| 1682 | } |
1673 | } |
| 1683 | */ |
1674 | |
| 1684 | LIMIT_MIN_MAX(tmp_int,MIN_GAS * 8,MAX_GAS * 8); |
1675 | LIMIT_MIN_MAX(tmp_int,MIN_GAS * 4,MAX_GAS * 4);test |
| 1685 | Motor[i].SetPoint = tmp_int / 8; |
1676 | Motor[i].SetPoint = tmp_int / 4; |
| 1686 | Motor[i].SetPointLowerBits = tmp_int % 8; |
1677 | Motor[i].SetPointLowerBits = tmp_int % 4; |
| 1687 | tmp_motorwert[i] = tmp_int; |
1678 | tmp_motorwert[i] = tmp_int; |
| 1688 | } |
1679 | } |
| 1689 | else |
1680 | else |