Rev 173 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 173 | Rev 188 | ||
---|---|---|---|
Line 695... | Line 695... | ||
695 | Mess_IntegralNick -= tmp_long; |
695 | Mess_IntegralNick -= tmp_long; |
696 | Mess_IntegralRoll -= tmp_long2; |
696 | Mess_IntegralRoll -= tmp_long2; |
697 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
697 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
698 | // Gieren |
698 | // Gieren |
699 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
699 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
700 | if(abs(StickGier) > 35) |
700 | if(abs(StickGier) > 20) // war 35 |
701 | { |
701 | { |
702 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
702 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
703 | } |
703 | } |
704 | tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx² |
704 | tmp_int = EE_Parameter.Gier_P * (StickGier * abs(StickGier)) / 512; // expo y = ax + bx² |
705 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
705 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
Line 847... | Line 847... | ||
847 | DebugOut.Analog[7] = GasMischanteil; |
847 | DebugOut.Analog[7] = GasMischanteil; |
Line 848... | Line 848... | ||
848 | 848 | ||
849 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
849 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
850 | // Gier-Anteil |
850 | // Gier-Anteil |
851 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
851 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
852 | #define MUL_G 1.4 |
852 | #define MUL_G 1.0 |
853 | GierMischanteil = MesswertGier - sollGier; // Regler für Gier |
853 | GierMischanteil = MesswertGier - sollGier; // Regler für Gier |
854 | //GierMischanteil = 0; |
854 | //GierMischanteil = 0; |
855 | if(GierMischanteil > (MUL_G * GasMischanteil)) GierMischanteil = MUL_G * GasMischanteil; |
855 | if(GierMischanteil > (MUL_G * GasMischanteil)) GierMischanteil = MUL_G * GasMischanteil; |
- | 856 | if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil); |
|
- | 857 | if(GierMischanteil > 100) GierMischanteil = 100; |
|
Line 856... | Line 858... | ||
856 | if(GierMischanteil < -(MUL_G * GasMischanteil)) GierMischanteil = -(MUL_G * GasMischanteil); |
858 | if(GierMischanteil < -100) GierMischanteil = -100; |
857 | 859 | ||
858 | if(GasMischanteil < 10) GierMischanteil = 0; |
860 | if(GasMischanteil < 20) GierMischanteil = 0; |
859 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
861 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
Line 860... | Line 862... | ||
860 | // Nick-Achse |
862 | // Nick-Achse |
Line 866... | Line 868... | ||
866 | if(SummeNick > 16000) SummeNick = 16000; |
868 | if(SummeNick > 16000) SummeNick = 16000; |
867 | if(SummeNick < -16000) SummeNick = -16000; |
869 | if(SummeNick < -16000) SummeNick = -16000; |
868 | pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
870 | pd_ergebnis = DiffNick;// + Ki * SummeNick; // PI-Regler für Nick |
869 | // Motor Vorn |
871 | // Motor Vorn |
870 | #define MUL 2 |
872 | #define MUL 2 |
871 | if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil); |
873 | if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
872 | if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil); |
874 | if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
Line 873... | Line 875... | ||
873 | 875 | ||
874 | motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer |
876 | motorwert = GasMischanteil + pd_ergebnis + GierMischanteil; // Mischer |
875 | if ((motorwert < 0)) motorwert = 0; |
877 | if ((motorwert < 0)) motorwert = 0; |
876 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
878 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
Line 889... | Line 891... | ||
889 | SummeRoll += DiffRoll; // I-Anteil |
891 | SummeRoll += DiffRoll; // I-Anteil |
890 | if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
892 | if(SummeRoll > 0) SummeRoll-= (abs(SummeRoll)/256 + 1); else SummeRoll += abs(SummeRoll)/256 + 1; |
891 | if(SummeRoll > 16000) SummeRoll = 16000; |
893 | if(SummeRoll > 16000) SummeRoll = 16000; |
892 | if(SummeRoll < -16000) SummeRoll = -16000; |
894 | if(SummeRoll < -16000) SummeRoll = -16000; |
893 | pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll |
895 | pd_ergebnis = DiffRoll;// + Ki * SummeRoll; // PI-Regler für Roll |
894 | if((pd_ergebnis > MUL * (GasMischanteil))) pd_ergebnis = MUL * (GasMischanteil); |
896 | if(pd_ergebnis > MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = MUL * (GasMischanteil + abs(GierMischanteil)); |
895 | if((pd_ergebnis < -MUL * (GasMischanteil))) pd_ergebnis = -MUL * (GasMischanteil); |
897 | if(pd_ergebnis < -MUL * (GasMischanteil + abs(GierMischanteil))) pd_ergebnis = -MUL * (GasMischanteil + abs(GierMischanteil)); |
896 | // Motor Links |
898 | // Motor Links |
897 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
899 | motorwert = GasMischanteil + pd_ergebnis - GierMischanteil; |
898 | if ((motorwert < 0)) motorwert = 0; |
900 | if ((motorwert < 0)) motorwert = 0; |
899 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
901 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
900 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
902 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |