Rev 614 | Rev 693 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 614 | Rev 623 | ||
---|---|---|---|
Line 729... | Line 729... | ||
729 | long tmp_long, tmp_long2; |
729 | long tmp_long, tmp_long2; |
730 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
730 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
731 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
731 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
732 | tmp_long /= 16; |
732 | tmp_long /= 16; |
733 | tmp_long2 /= 16; |
733 | tmp_long2 /= 16; |
- | 734 | if((MaxStickNick > 15) || (MaxStickRoll > 15)) |
|
- | 735 | { |
|
- | 736 | tmp_long /= 3; |
|
- | 737 | tmp_long2 /= 3; |
|
- | 738 | } |
|
734 | if((MaxStickNick > 15) || (MaxStickRoll > 15) || (abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25)) |
739 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
735 | { |
740 | { |
736 | tmp_long /= 3; |
741 | tmp_long /= 3; |
737 | tmp_long2 /= 3; |
742 | tmp_long2 /= 3; |
738 | } |
743 | } |
Line 780... | Line 785... | ||
780 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
785 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
781 | // Gyro-Drift ermitteln |
786 | // Gyro-Drift ermitteln |
782 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
787 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
783 | MittelIntegralNick2 /= ABGLEICH_ANZAHL; |
788 | MittelIntegralNick2 /= ABGLEICH_ANZAHL; |
784 | MittelIntegralRoll2 /= ABGLEICH_ANZAHL; |
789 | MittelIntegralRoll2 /= ABGLEICH_ANZAHL; |
785 | // tmp_long = (long)(MittelIntegralNick2 - (long)IntegralAccNick); |
- | |
786 | // tmp_long2 = (long)(MittelIntegralRoll2 - (long)IntegralAccRoll); |
- | |
787 | tmp_long = IntegralNick2 - IntegralNick; |
790 | tmp_long = IntegralNick2 - IntegralNick; |
788 | tmp_long2 = IntegralRoll2 - IntegralRoll; |
791 | tmp_long2 = IntegralRoll2 - IntegralRoll; |
789 | //DebugOut.Analog[25] = MittelIntegralRoll2 / 26; |
792 | //DebugOut.Analog[25] = MittelIntegralRoll2 / 26; |
Line 790... | Line 793... | ||
790 | 793 | ||
Line 873... | Line 876... | ||
873 | { |
876 | { |
874 | cnt = 0; |
877 | cnt = 0; |
875 | } |
878 | } |
Line 876... | Line 879... | ||
876 | 879 | ||
877 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
- | |
878 | //if(cnt > 1) beeptime = 50; |
880 | if(cnt > EE_Parameter.Driftkomp) cnt = EE_Parameter.Driftkomp; |
879 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
881 | if(IntegralFehlerRoll > FEHLER_LIMIT) AdNeutralRoll += cnt; |
880 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
882 | if(IntegralFehlerRoll < -FEHLER_LIMIT) AdNeutralRoll -= cnt; |
881 | DebugOut.Analog[27] = ausgleichRoll; |
883 | DebugOut.Analog[27] = ausgleichRoll; |
882 | DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
884 | DebugOut.Analog[23] = AdNeutralNick;//10*(AdNeutralNick - StartNeutralNick); |
Line 886... | Line 888... | ||
886 | { |
888 | { |
887 | LageKorrekturRoll = 0; |
889 | LageKorrekturRoll = 0; |
888 | LageKorrekturNick = 0; |
890 | LageKorrekturNick = 0; |
889 | } |
891 | } |
Line 890... | Line -... | ||
890 | - | ||
891 | // TEST |
- | |
892 | // LageKorrekturRoll = 0; |
- | |
893 | // LageKorrekturNick = 0; |
- | |
894 | // TEST |
- | |
895 | 892 | ||
896 | if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH |
893 | if(!IntegralFaktor) { LageKorrekturRoll = 0; LageKorrekturNick = 0;} // z.B. bei HH |
897 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
894 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++ |
898 | MittelIntegralNick_Alt = MittelIntegralNick; |
895 | MittelIntegralNick_Alt = MittelIntegralNick; |
899 | MittelIntegralRoll_Alt = MittelIntegralRoll; |
896 | MittelIntegralRoll_Alt = MittelIntegralRoll; |
Line 999... | Line 996... | ||
999 | 996 | ||
1000 | if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; |
997 | if(Looping_Nick) MesswertNick = MesswertNick * GyroFaktor; |
1001 | else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
998 | else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
1002 | if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
999 | if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
1003 | else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
1000 | else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
Line 1004... | Line 1001... | ||
1004 | MesswertGier = MesswertGier * (GyroFaktor) + Integral_Gier * IntegralFaktor/2; |
1001 | MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; |
1005 | 1002 | ||
1006 | DebugOut.Analog[25] = IntegralRoll * IntegralFaktor; |
1003 | DebugOut.Analog[25] = IntegralRoll * IntegralFaktor; |
Line 1083... | Line 1080... | ||
1083 | // Nick-Achse |
1080 | // Nick-Achse |
1084 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1081 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1085 | DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
1082 | DiffNick = MesswertNick - (StickNick - GPS_Nick); // Differenz bestimmen |
1086 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
1083 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - (StickNick - GPS_Nick); // I-Anteil bei Winkelregelung |
1087 | else SummeNick += DiffNick; // I-Anteil bei HH |
1084 | else SummeNick += DiffNick; // I-Anteil bei HH |
1088 | // if(SummeNick > 0) SummeNick-= 2 ; else SummeNick += 2 ; |
- | |
1089 | if(SummeNick > 16000) SummeNick = 16000; |
1085 | if(SummeNick > 16000) SummeNick = 16000; |
1090 | if(SummeNick < -16000) SummeNick = -16000; |
1086 | if(SummeNick < -16000) SummeNick = -16000; |
1091 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
1087 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
1092 | // Motor Vorn |
1088 | // Motor Vorn |
1093 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1089 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
Line 1109... | Line 1105... | ||
1109 | // Roll-Achse |
1105 | // Roll-Achse |
1110 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1106 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1111 | DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1107 | DiffRoll = MesswertRoll - (StickRoll - GPS_Roll); // Differenz bestimmen |
1112 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1108 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - (StickRoll - GPS_Roll);// I-Anteil bei Winkelregelung |
1113 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1109 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1114 | // if(SummeRoll > 0) SummeRoll-= 2 ; else SummeRoll += 2 ; |
- | |
1115 | if(SummeRoll > 16000) SummeRoll = 16000; |
1110 | if(SummeRoll > 16000) SummeRoll = 16000; |
1116 | if(SummeRoll < -16000) SummeRoll = -16000; |
1111 | if(SummeRoll < -16000) SummeRoll = -16000; |
1117 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1112 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1118 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1113 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1119 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
1114 | if(pd_ergebnis > tmp_int) pd_ergebnis = tmp_int; |
Line 1123... | Line 1118... | ||
1123 | #define GRENZE Poti1 |
1118 | #define GRENZE Poti1 |
Line 1124... | Line 1119... | ||
1124 | 1119 | ||
1125 | if ((motorwert < 0)) motorwert = 0; |
1120 | if ((motorwert < 0)) motorwert = 0; |
1126 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
1121 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
1127 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
- | |
1128 | Motor_Links = motorwert; |
- | |
1129 | /* |
- | |
1130 | if(motorwert > (int)Motor_Links + GRENZE) Motor_Links += GRENZE; |
- | |
1131 | else if(motorwert < (int)Motor_Links - GRENZE) Motor_Links -= GRENZE; |
1122 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1132 | else Motor_Links = motorwert; |
- | |
1133 | */ |
1123 | Motor_Links = motorwert; |
1134 | // Motor Rechts |
1124 | // Motor Rechts |
Line 1135... | Line 1125... | ||
1135 | motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; |
1125 | motorwert = GasMischanteil - pd_ergebnis - GierMischanteil; |
1136 | 1126 | ||
1137 | if ((motorwert < 0)) motorwert = 0; |
1127 | if ((motorwert < 0)) motorwert = 0; |
1138 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
- | |
1139 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
- | |
1140 | Motor_Rechts = motorwert; |
- | |
1141 | /* |
- | |
1142 | if(motorwert > Motor_Rechts + GRENZE) Motor_Rechts += GRENZE; |
1128 | else if(motorwert > MAX_GAS) motorwert = MAX_GAS; |
1143 | else if(motorwert < Motor_Rechts - GRENZE) Motor_Rechts -= GRENZE; |
- | |
1144 | else Motor_Rechts = motorwert; |
1129 | if (motorwert < MIN_GAS) motorwert = MIN_GAS; |
1145 | */ |
1130 | Motor_Rechts = motorwert; |