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; |