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