Rev 1680 | Rev 1683 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1680 | Rev 1682 | ||
---|---|---|---|
Line 177... | Line 177... | ||
177 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
177 | DebugOut.Analog[6] = (Mess_Integral_Hoch / 512);//AdWertAccHoch;// Aktuell_az;//(Mess_Integral_Hoch / 512);//Aktuell_az; |
178 | DebugOut.Analog[8] = KompassValue; |
178 | DebugOut.Analog[8] = KompassValue; |
179 | DebugOut.Analog[9] = UBat; |
179 | DebugOut.Analog[9] = UBat; |
180 | DebugOut.Analog[10] = SenderOkay; |
180 | DebugOut.Analog[10] = SenderOkay; |
181 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
181 | DebugOut.Analog[11] = ErsatzKompass / GIER_GRAD_FAKTOR; |
182 | DebugOut.Analog[16] = Capacity.MinOfMaxPWM; |
- | |
183 | DebugOut.Analog[20] = ServoNickValue; |
182 | DebugOut.Analog[20] = ServoNickValue; |
184 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
183 | DebugOut.Analog[22] = Capacity.ActualCurrent; |
185 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
184 | DebugOut.Analog[23] = Capacity.UsedCapacity; |
186 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
185 | // DebugOut.Analog[22] = FromNaviCtrl_Value.GpsZ; |
187 | // DebugOut.Analog[24] = MesswertNick/2; |
186 | // DebugOut.Analog[24] = MesswertNick/2; |
188 | // DebugOut.Analog[25] = MesswertRoll/2; |
187 | // DebugOut.Analog[25] = MesswertRoll/2; |
189 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
188 | // DebugOut.Analog[27] = (int)FromNaviCtrl_Value.Kalman_MaxDrift; |
190 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
189 | // DebugOut.Analog[28] = (int)FromNaviCtrl_Value.Kalman_MaxFusion; |
191 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
190 | // DebugOut.Analog[29] = (int)FromNaviCtrl_Value.Kalman_K; |
192 | //DebugOut.Analog[28] = I2CError; |
191 | //DebugOut.Analog[28] = I2CError; |
193 | DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
192 | // DebugOut.Analog[29] = FromNaviCtrl_Value.SerialDataOkay; |
- | 193 | DebugOut.Analog[29] = Capacity.MinOfMaxPWM; |
|
194 | DebugOut.Analog[30] = GPS_Nick; |
194 | DebugOut.Analog[30] = GPS_Nick; |
195 | DebugOut.Analog[31] = GPS_Roll; |
195 | DebugOut.Analog[31] = GPS_Roll; |
196 | } |
196 | } |
Line 197... | Line 197... | ||
197 | 197 | ||
Line 592... | Line 592... | ||
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; |
595 | if(tmp > 50 && NaviDataOkay > 200) |
595 | if(tmp > 50 && NaviDataOkay > 200) |
- | 596 | { |
|
- | 597 | #ifdef SWITCH_LEARNS_CAREFREE |
|
- | 598 | if(!CareFree) ControlHeading = (((int) EE_Parameter.OrientationAngle * 15 + KompassValue) % 360) / 2; |
|
596 | { |
599 | #endif |
597 | CareFree = 1; |
600 | CareFree = 1; |
598 | if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0; |
601 | if(tmp >= 248 && Poti[255 - tmp] < 50) CareFree = 0; |
599 | } |
602 | } |
Line 600... | Line 603... | ||
600 | else CareFree = 0; |
603 | else CareFree = 0; |
Line 667... | Line 670... | ||
667 | } |
670 | } |
668 | if((modell_fliegt < 256)) |
671 | if((modell_fliegt < 256)) |
669 | { |
672 | { |
670 | SummeNick = 0; |
673 | SummeNick = 0; |
671 | SummeRoll = 0; |
674 | SummeRoll = 0; |
- | 675 | sollGier = 0; |
|
- | 676 | Mess_Integral_Gier = 0; |
|
672 | if(modell_fliegt == 250) |
677 | if(modell_fliegt == 250) |
673 | { |
678 | { |
674 | NeueKompassRichtungMerken = 1; |
679 | NeueKompassRichtungMerken = 1; |
675 | sollGier = 0; |
- | |
676 | Mess_Integral_Gier = 0; |
- | |
677 | // Mess_Integral_Gier2 = 0; |
680 | // Mess_Integral_Gier2 = 0; |
678 | } |
681 | } |
679 | } else FCFlags |= FCFLAG_FLY; |
682 | } else FCFlags |= FCFLAG_FLY; |
Line 680... | Line 683... | ||
680 | 683 | ||
Line 1617... | Line 1620... | ||
1617 | if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
1620 | if(GierMischanteil < -((tmp_int - GasMischanteil))) GierMischanteil = -((tmp_int - GasMischanteil)); |
Line 1618... | Line 1621... | ||
1618 | 1621 | ||
1619 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1622 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1620 | // Nick-Achse |
1623 | // Nick-Achse |
- | 1624 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
1621 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1625 | #define ABSCHWAECHEN 2 |
1622 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1626 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1623 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1627 | if(IntegralFaktor) SummeNick += IntegralNickMalFaktor - StickNick; // I-Anteil bei Winkelregelung |
1624 | else SummeNick += DiffNick; // I-Anteil bei HH |
1628 | else SummeNick += DiffNick; // I-Anteil bei HH |
1625 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
1629 | if(SummeNick > (STICK_GAIN * 16000L)) SummeNick = (STICK_GAIN * 16000L); |
- | 1630 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
|
- | 1631 | if(Parameter_UserParam5 > 50) |
|
- | 1632 | pd_ergebnis_nick = DiffNick/ABSCHWAECHEN + SummeNick / Ki; // PI-Regler für Nick |
|
1626 | if(SummeNick < -(16000L * STICK_GAIN)) SummeNick = -(16000L * STICK_GAIN); |
1633 | else |
- | 1634 | pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
|
1627 | pd_ergebnis_nick = DiffNick + SummeNick / Ki; // PI-Regler für Nick |
1635 | |
1628 | // Motor Vorn |
1636 | // Motor Vorn |
1629 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1637 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1630 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
1638 | if(pd_ergebnis_nick > tmp_int) pd_ergebnis_nick = tmp_int; |
Line 1636... | Line 1644... | ||
1636 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1644 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1637 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1645 | if(IntegralFaktor) SummeRoll += IntegralRollMalFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1638 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1646 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1639 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1647 | if(SummeRoll > (STICK_GAIN * 16000L)) SummeRoll = (STICK_GAIN * 16000L); |
1640 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
1648 | if(SummeRoll < -(16000L * STICK_GAIN)) SummeRoll = -(16000L * STICK_GAIN); |
- | 1649 | ||
- | 1650 | if(Parameter_UserParam5 > 50) |
|
- | 1651 | pd_ergebnis_roll = DiffRoll/ABSCHWAECHEN + SummeRoll / Ki; // PI-Regler für Roll |
|
- | 1652 | else |
|
1641 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
1653 | pd_ergebnis_roll = DiffRoll + SummeRoll / Ki; // PI-Regler für Roll |
- | 1654 | ||
1642 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1655 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1643 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1656 | if(pd_ergebnis_roll > tmp_int) pd_ergebnis_roll = tmp_int; |
1644 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
1657 | if(pd_ergebnis_roll < -tmp_int) pd_ergebnis_roll = -tmp_int; |
Line -... | Line 1658... | ||
- | 1658 | ||
- | 1659 | DebugOut.Analog[17] = SummeRoll / 4; |
|
- | 1660 | DebugOut.Analog[16] = SummeNick / 4; |
|
- | 1661 | DebugOut.Analog[18] = Mess_Integral_Gier/2; |
|
1645 | 1662 | ||
1646 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1663 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1647 | // Universal Mixer |
1664 | // Universal Mixer |
1648 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1665 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1649 | for(i=0; i<MAX_MOTORS; i++) |
1666 | for(i=0; i<MAX_MOTORS; i++) |