Rev 1093 | Rev 1099 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1093 | Rev 1096 | ||
---|---|---|---|
Line 289... | Line 289... | ||
289 | NaviAccRoll += AdWertAccRoll; |
289 | NaviAccRoll += AdWertAccRoll; |
290 | NaviCntAcc++; |
290 | NaviCntAcc++; |
291 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
291 | IntegralAccZ += Aktuell_az - NeutralAccZ; |
292 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
292 | // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
293 | ErsatzKompass += MesswertGier; |
293 | ErsatzKompass += MesswertGier; |
294 | //Salvo 12.11.2007 |
294 | //Salvo 29.12.2008 |
- | 295 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
|
- | 296 | { |
|
295 | GyroKomp_Int += (long)MesswertGier; |
297 | GyroKomp_Int += (long)MesswertGier; |
296 | GyroGier_Comp += (long)MesswertGier; |
298 | GyroGier_Comp += (long)MesswertGier; |
- | 299 | } |
|
- | 300 | ||
297 | //Salvo End |
301 | //Salvo End |
298 | Mess_Integral_Gier += MesswertGier; |
302 | Mess_Integral_Gier += MesswertGier; |
299 | // Mess_Integral_Gier2 += MesswertGier; |
303 | // Mess_Integral_Gier2 += MesswertGier; |
300 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
304 | if(ErsatzKompass >= (360L * GIER_GRAD_FAKTOR)) ErsatzKompass -= 360L * GIER_GRAD_FAKTOR; // 360° Umschlag |
301 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
305 | if(ErsatzKompass < 0) ErsatzKompass += 360L * GIER_GRAD_FAKTOR; |
Line 966... | Line 970... | ||
966 | Mess_IntegralNick2 -= IntegralFehlerNick; |
970 | Mess_IntegralNick2 -= IntegralFehlerNick; |
967 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
971 | Mess_IntegralRoll2 -= IntegralFehlerRoll; |
Line 968... | Line 972... | ||
968 | 972 | ||
969 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
973 | // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
- | 974 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
|
- | 975 | ||
970 | // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
976 | //Salvo 29.12.2008 auskommentiert |
971 | if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
977 | // if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
- | 978 | // if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
|
Line 972... | Line 979... | ||
972 | if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
979 | //Salvo End |
973 | 980 | ||
974 | //DebugOut.Analog[22] = MittelIntegralRoll / 26; |
981 | //DebugOut.Analog[22] = MittelIntegralRoll / 26; |
Line 1095... | Line 1102... | ||
1095 | } |
1102 | } |
1096 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
1103 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
1097 | // Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
1104 | // Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
1098 | if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
1105 | if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
1099 | { |
1106 | { |
1100 | Kompass_Neuer_Wert = 0; |
1107 | Kompass_Neuer_Wert = 0; |
1101 | w = (abs(Mittelwert_AccNick)); |
1108 | w = (abs(Mittelwert_AccNick)); |
1102 | v = (abs(Mittelwert_AccRoll)); |
1109 | v = (abs(Mittelwert_AccRoll)); |
1103 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
1110 | if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
1104 | { |
1111 | { |
1105 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
1112 | if ((abs(KompassValue - Kompass_Value_Old)) <= 5) // Aufeinanderfolgende Werte duerfen nur minimal abweichen |
1106 | { |
1113 | { |
Line 1107... | Line 1114... | ||
1107 | 1114 | ||
1108 | if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
1115 | if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
1109 | { |
1116 | { |
1110 | if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
1117 | if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
1111 | if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da |
1118 | if (cnt_stickgier_zero > 10) // nur Abgleichen wenn keine Stickbewegung da |
1112 | { |
1119 | { |
1113 | w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
1120 | w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
1114 | v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
1121 | v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
1115 | if (v <-180) v +=360; // Uberlaufkorrektur |
1122 | if (v <-180) v +=360; // Uberlaufkorrektur |
Line 1164... | Line 1171... | ||
1164 | magkompass_ok = 0; |
1171 | magkompass_ok = 0; |
1165 | GyroGier_Comp = 0; |
1172 | GyroGier_Comp = 0; |
1166 | } |
1173 | } |
1167 | Kompass_Value_Old = KompassValue; |
1174 | Kompass_Value_Old = KompassValue; |
1168 | } |
1175 | } |
- | 1176 | else magkompass_ok = 0; |
|
- | 1177 | ||
1169 | // Salvo End ************************* |
1178 | // Salvo End ************************* |
Line 1170... | Line 1179... | ||
1170 | 1179 | ||
1171 | // Salvo 6.10.2007 |
1180 | // Salvo 6.10.2007 |
1172 | // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
1181 | // GPS Home aktivieren wenn Knueppel in Ruhelage und Hoehenschalter aktiviert ist |
Line 1210... | Line 1219... | ||
1210 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1219 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1211 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
1220 | // if(GasMischanteil < 35) { if(StickGier > 10) StickGier = 10; else if(StickGier < -10) StickGier = -10;}; |
1212 | if(abs(StickGier) > 15) // war 35 |
1221 | if(abs(StickGier) > 15) // war 35 |
1213 | { |
1222 | { |
1214 | KompassSignalSchlecht = 1000; |
1223 | KompassSignalSchlecht = 1000; |
1215 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) |
1224 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
1216 | { |
- | |
1217 | NeueKompassRichtungMerken = 1; |
- | |
1218 | }; |
- | |
1219 | } |
1225 | } |
1220 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1226 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1221 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
1227 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
1222 | sollGier = tmp_int; |
1228 | sollGier = tmp_int; |
1223 | Mess_Integral_Gier -= tmp_int; |
1229 | Mess_Integral_Gier -= tmp_int; |
Line 1227... | Line 1233... | ||
1227 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1233 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1228 | // Kompass |
1234 | // Kompass |
1229 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1235 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1230 | //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
1236 | //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
Line 1231... | Line 1237... | ||
1231 | 1237 | ||
1232 | // Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert |
1238 | // Salvo 29.12.2008 |
1233 | if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
1239 | if (NeueKompassRichtungMerken) |
1234 | { |
1240 | { |
1235 | KompassStartwert = KompassValue; |
1241 | KompassStartwert = KompassValue; |
1236 | NeueKompassRichtungMerken = 0; |
1242 | NeueKompassRichtungMerken = 0; |
1237 | } |
1243 | } |
Line 1241... | Line 1247... | ||
1241 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
1247 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
1242 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
1248 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
1243 | if(w >= 0) |
1249 | if(w >= 0) |
1244 | { |
1250 | { |
1245 | // Salvo Kompasssteuerung ********************** |
1251 | // Salvo Kompasssteuerung ********************** |
1246 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
1252 | if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten wenn dieser ok ist |
1247 | // Salvo End |
1253 | // Salvo End |
1248 | } |
1254 | } |
Line 1249... | Line 1255... | ||
1249 | 1255 | ||
Line 1290... | Line 1296... | ||
1290 | DebugOut.Analog[25] = debug_gp_2; |
1296 | DebugOut.Analog[25] = debug_gp_2; |
1291 | DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
1297 | DebugOut.Analog[26] = gps_rel_act_position.utm_east; //in 10cm ausgeben |
1292 | DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
1298 | DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
1293 | DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
1299 | DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
1294 | DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
1300 | DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
- | 1301 | // DebugOut.Analog[29] = magkompass_ok; |
|
- | 1302 | ||
Line 1295... | Line 1303... | ||
1295 | 1303 | ||
1296 | DebugOut.Analog[30] = GPS_Nick; |
1304 | DebugOut.Analog[30] = GPS_Nick; |
- | 1305 | DebugOut.Analog[31] = GPS_Roll; |
|
- | 1306 | // DebugOut.Analog[30] = KompassStartwert; |
|
Line 1297... | Line 1307... | ||
1297 | DebugOut.Analog[31] = GPS_Roll; |
1307 | // DebugOut.Analog[31] = KompassRichtung; |
1298 | 1308 | ||
1299 | 1309 |