Rev 847 | Rev 855 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
| Rev 847 | Rev 854 | ||
|---|---|---|---|
| Line 1044... | Line 1044... | ||
| 1044 | DebugOut.Analog[16] = Mittelwert_AccHoch; |
1044 | DebugOut.Analog[16] = Mittelwert_AccHoch; |
| Line 1045... | Line 1045... | ||
| 1045 | 1045 | ||
| 1046 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
1046 | //DebugOut.Analog[17] = FromNaviCtrl_Value.Distance; |
| 1047 | //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
1047 | //DebugOut.Analog[18] = (int)FromNaviCtrl_Value.OsdBar; |
| - | 1048 | DebugOut.Analog[19] = WinkelOut.CalcState; |
|
| Line 1048... | Line 1049... | ||
| 1048 | DebugOut.Analog[19] = WinkelOut.CalcState; |
1049 | DebugOut.Analog[20] = ServoValue; |
| 1049 | 1050 | ||
| Line 1086... | Line 1087... | ||
| 1086 | else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
1087 | else MesswertNick = IntegralNick * IntegralFaktor + MesswertNick * GyroFaktor; |
| 1087 | if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
1088 | if(Looping_Roll) MesswertRoll = MesswertRoll * GyroFaktor; |
| 1088 | else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
1089 | else MesswertRoll = IntegralRoll * IntegralFaktor + MesswertRoll * GyroFaktor; |
| 1089 | MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; |
1090 | MesswertGier = MesswertGier * (2 * GyroFaktor) + Integral_Gier * IntegralFaktor / 2; |
| Line -... | Line 1091... | ||
| - | 1091 | ||
| - | 1092 | DebugOut.Analog[21] = MesswertNick; |
|
| - | 1093 | DebugOut.Analog[22] = MesswertRoll; |
|
| 1090 | 1094 | ||
| 1091 | // Maximalwerte abfangen |
1095 | // Maximalwerte abfangen |
| 1092 | #define MAX_SENSOR 2048 |
1096 | #define MAX_SENSOR 2048 |
| 1093 | if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; |
1097 | if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; |
| 1094 | if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR; |
1098 | if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR; |
| Line 1171... | Line 1175... | ||
| 1171 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1175 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1172 | // Nick-Achse |
1176 | // Nick-Achse |
| 1173 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1177 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1174 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
1178 | DiffNick = MesswertNick - StickNick; // Differenz bestimmen |
| 1175 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung |
1179 | if(IntegralFaktor) SummeNick += IntegralNick * IntegralFaktor - StickNick; // I-Anteil bei Winkelregelung |
| 1176 | - | ||
| 1177 | else SummeNick += DiffNick; // I-Anteil bei HH |
1180 | else SummeNick += DiffNick; // I-Anteil bei HH |
| 1178 | if(SummeNick > 16000) SummeNick = 16000; |
1181 | if(SummeNick > 16000) SummeNick = 16000; |
| 1179 | if(SummeNick < -16000) SummeNick = -16000; |
1182 | if(SummeNick < -16000) SummeNick = -16000; |
| 1180 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
1183 | pd_ergebnis = DiffNick + Ki * SummeNick; // PI-Regler für Nick |
| 1181 | // Motor Vorn |
1184 | // Motor Vorn |
| Line 1197... | Line 1200... | ||
| 1197 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1200 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1198 | // Roll-Achse |
1201 | // Roll-Achse |
| 1199 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1202 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| 1200 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
1203 | DiffRoll = MesswertRoll - StickRoll; // Differenz bestimmen |
| 1201 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
1204 | if(IntegralFaktor) SummeRoll += IntegralRoll * IntegralFaktor - StickRoll;// I-Anteil bei Winkelregelung |
| 1202 | - | ||
| 1203 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
1205 | else SummeRoll += DiffRoll; // I-Anteil bei HH |
| 1204 | if(SummeRoll > 16000) SummeRoll = 16000; |
1206 | if(SummeRoll > 16000) SummeRoll = 16000; |
| 1205 | if(SummeRoll < -16000) SummeRoll = -16000; |
1207 | if(SummeRoll < -16000) SummeRoll = -16000; |
| 1206 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
1208 | pd_ergebnis = DiffRoll + Ki * SummeRoll; // PI-Regler für Roll |
| 1207 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |
1209 | tmp_int = (long)((long)Parameter_DynamicStability * (long)(GasMischanteil + abs(GierMischanteil)/2)) / 64; |