Rev 1648 | Rev 1653 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 1648 | Rev 1652 | ||
|---|---|---|---|
| Line 99... | Line 99... | ||
| 99 | unsigned char RequiredMotors = 0; |
99 | unsigned char RequiredMotors = 0; |
| 100 | long HoehenWert = 0; |
100 | long HoehenWert = 0; |
| 101 | long SollHoehe = 0; |
101 | long SollHoehe = 0; |
| 102 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
102 | int LageKorrekturRoll = 0,LageKorrekturNick = 0; |
| 103 | //float Ki = FAKTOR_I; |
103 | //float Ki = FAKTOR_I; |
| 104 | int Ki = 10300 / 33; |
104 | int Ki = (10300/2) / 33; |
| 105 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
105 | unsigned char Looping_Nick = 0,Looping_Roll = 0; |
| 106 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
106 | unsigned char Looping_Links = 0, Looping_Rechts = 0, Looping_Unten = 0, Looping_Oben = 0; |
| Line 107... | Line 107... | ||
| 107 | 107 | ||
| 108 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
108 | unsigned char Parameter_Luftdruck_D = 48; // Wert : 0-250 |
| Line 162... | Line 162... | ||
| 162 | #define LIMIT_MIN(value, min) {if(value <= min) value = min;} |
162 | #define LIMIT_MIN(value, min) {if(value <= min) value = min;} |
| 163 | #define LIMIT_MAX(value, max) {if(value >= max) value = max;} |
163 | #define LIMIT_MAX(value, max) {if(value >= max) value = max;} |
| 164 | #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
164 | #define LIMIT_MIN_MAX(value, min, max) {if(value <= min) value = min; else if(value >= max) value = max;} |
| Line 165... | Line -... | ||
| 165 | - | ||
| 166 | - | ||
| 167 | int MotorSmoothing(int neu, int alt) |
- | |
| 168 | { |
- | |
| 169 | int motor; |
- | |
| 170 | if(neu > alt) motor = (1*(int)alt + neu) / 2; |
- | |
| 171 | else motor = neu - (alt - neu)*1; |
- | |
| 172 | //if(Poti2 < 20) return(neu); |
- | |
| 173 | return(motor); |
- | |
| 174 | } |
165 | |
| 175 | 166 | ||
| 176 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
167 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 177 | // Debugwerte zuordnen |
168 | // Debugwerte zuordnen |
| 178 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
169 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 566... | Line 557... | ||
| 566 | //############################################################################ |
557 | //############################################################################ |
| 567 | // Trägt ggf. das Poti als Parameter ein |
558 | // Trägt ggf. das Poti als Parameter ein |
| 568 | void ParameterZuordnung(void) |
559 | void ParameterZuordnung(void) |
| 569 | //############################################################################ |
560 | //############################################################################ |
| 570 | { |
561 | { |
| - | 562 | ||
| 571 | #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];} |
563 | #define CHK_POTI(b,a) {if(a < 248) b = a; else b = Poti[255 - a];} |
| 572 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);} |
564 | #define CHK_POTI_MM(b,a,min,max) {CHK_POTI(b,a); LIMIT_MIN_MAX(b, min, max);} |
| Line 573... | Line 565... | ||
| 573 | 565 | ||
| 574 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
566 | CHK_POTI_MM(Parameter_Luftdruck_D,EE_Parameter.Luftdruck_D,0,100); |
| Line 604... | Line 596... | ||
| 604 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2); |
596 | CHK_POTI(Parameter_AchsKopplung2, EE_Parameter.AchsKopplung2); |
| 605 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
597 | CHK_POTI(Parameter_CouplingYawCorrection,EE_Parameter.CouplingYawCorrection); |
| 606 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
598 | // CHK_POTI(Parameter_AchsGegenKopplung1,EE_Parameter.AchsGegenKopplung1,0,255); |
| 607 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
599 | CHK_POTI(Parameter_DynamicStability,EE_Parameter.DynamicStability); |
| 608 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
600 | CHK_POTI(Parameter_ExternalControl,EE_Parameter.ExternalControl); |
| 609 | Ki = 10300 / (Parameter_I_Faktor + 1); |
601 | Ki = (10300 / 2) / (Parameter_I_Faktor + 1); |
| 610 | MAX_GAS = EE_Parameter.Gas_Max; |
602 | MAX_GAS = EE_Parameter.Gas_Max; |
| 611 | MIN_GAS = EE_Parameter.Gas_Min; |
603 | MIN_GAS = EE_Parameter.Gas_Min; |
| 612 | } |
604 | } |
| Line 613... | Line 605... | ||
| 613 | 605 | ||
| Line 1615... | Line 1607... | ||
| 1615 | else SummeNick += DiffNick; // I-Anteil bei HH |
1607 | else SummeNick += DiffNick; // I-Anteil bei HH |
| 1616 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1608 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
| 1617 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1609 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
| 1618 | pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
1610 | pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
| 1619 | // Motor Vorn |
1611 | // Motor Vorn |
| 1620 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1612 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / (64/2); |
| 1621 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1613 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
| 1622 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
1614 | if(pd_ergebnis_nick < -tmp_int) pd_ergebnis_nick = -tmp_int; |
| Line 1623... | Line 1615... | ||
| 1623 | 1615 | ||
| 1624 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1616 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 1628... | Line 1620... | ||
| 1628 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1620 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
| 1629 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1621 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
| 1630 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1622 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
| 1631 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1623 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
| 1632 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
1624 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
| 1633 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1625 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / (64/2); |
| 1634 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1626 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
| 1635 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1627 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
| Line 1636... | Line 1628... | ||
| 1636 | 1628 | ||
| 1637 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1629 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| Line 1640... | Line 1632... | ||
| 1640 | for(i=0; i<MAX_MOTORS; i++) |
1632 | for(i=0; i<MAX_MOTORS; i++) |
| 1641 | { |
1633 | { |
| 1642 | signed int tmp_int; |
1634 | signed int tmp_int; |
| 1643 | if(Mixer.Motor[i][0] > 0) |
1635 | if(Mixer.Motor[i][0] > 0) |
| 1644 | { |
1636 | { |
| - | 1637 | // Gas |
|
| 1645 | tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 64L; |
1638 | if(Mixer.Motor[i][0] == 64) tmp_int = GasMischanteil * 2; else tmp_int = ((long)GasMischanteil * Mixer.Motor[i][0]) / 32L; |
| - | 1639 | // Nick |
|
| - | 1640 | if(Mixer.Motor[i][1] == 64) tmp_int += pd_ergebnis_nick; |
|
| - | 1641 | else if(Mixer.Motor[i][1] == -64) tmp_int -= pd_ergebnis_nick; |
|
| 1646 | tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
1642 | else tmp_int += ((long)pd_ergebnis_nick * Mixer.Motor[i][1]) / 64L; |
| - | 1643 | // Roll |
|
| - | 1644 | if(Mixer.Motor[i][2] == 64) tmp_int += pd_ergebnis_roll; |
|
| - | 1645 | else if(Mixer.Motor[i][2] == -64) tmp_int -= pd_ergebnis_roll; |
|
| 1647 | tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
1646 | else tmp_int += ((long)pd_ergebnis_roll * Mixer.Motor[i][2]) / 64L; |
| - | 1647 | // Gier |
|
| - | 1648 | if(Mixer.Motor[i][3] == 64) tmp_int += GierMischanteil * 2; |
|
| - | 1649 | else if(Mixer.Motor[i][3] == -64) tmp_int -= GierMischanteil * 2; |
|
| 1648 | tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 64L; |
1650 | else tmp_int += ((long)GierMischanteil * Mixer.Motor[i][3]) / 32L; |
| - | 1651 | ||
| 1649 | tmp_motorwert[i] = MotorSmoothing(tmp_int,tmp_motorwert[i]); // Filter |
1652 | if(tmp_int > tmp_motorwert[i]) tmp_int = (tmp_motorwert[i] + tmp_int) / 2; // MotorSmoothing |
| 1650 | tmp_int = tmp_motorwert[i] / 4; |
- | |
| 1651 | Motor[i].SetPointLowerBits = tmp_motorwert[i] % 4; |
1653 | else tmp_int = 2 * tmp_int - tmp_motorwert[i]; // MotorSmoothing |
| - | 1654 | ||
| 1652 | LIMIT_MIN_MAX(tmp_int,MIN_GAS,MAX_GAS); |
1655 | LIMIT_MIN_MAX(tmp_int,MIN_GAS * 8,MAX_GAS * 8); |
| 1653 | Motor[i].SetPoint = tmp_int; |
1656 | Motor[i].SetPoint = tmp_int / 8; |
| - | 1657 | Motor[i].SetPointLowerBits = tmp_int % 8; |
|
| - | 1658 | tmp_motorwert[i] = tmp_int; |
|
| 1654 | } |
1659 | } |
| 1655 | else |
1660 | else |
| 1656 | { |
1661 | { |
| 1657 | Motor[i].SetPoint = 0; |
1662 | Motor[i].SetPoint = 0; |
| 1658 | Motor[i].SetPointLowerBits = 0; |
1663 | Motor[i].SetPointLowerBits = 0; |