| 291,9 → 291,13 |
|---|
| IntegralAccZ += Aktuell_az - NeutralAccZ; |
| // Gier ++++++++++++++++++++++++++++++++++++++++++++++++ |
| ErsatzKompass += MesswertGier; |
| //Salvo 12.11.2007 |
| //Salvo 29.12.2008 |
| if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
| { |
| GyroKomp_Int += (long)MesswertGier; |
| GyroGier_Comp += (long)MesswertGier; |
| } |
| |
| //Salvo End |
| Mess_Integral_Gier += MesswertGier; |
| // Mess_Integral_Gier2 += MesswertGier; |
| 968,9 → 972,12 |
|---|
| |
| // IntegralFehlerNick = (IntegralFehlerNick * 1 + tmp_long) / 2; |
| // IntegralFehlerRoll = (IntegralFehlerRoll * 1 + tmp_long2) / 2; |
| if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
| if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
| |
| //Salvo 29.12.2008 auskommentiert |
| // if(GierGyroFehler > ABGLEICH_ANZAHL/2) { AdNeutralGier++; AdNeutralGierBias++; } |
| // if(GierGyroFehler <-ABGLEICH_ANZAHL/2) { AdNeutralGier--; AdNeutralGierBias--; } |
| //Salvo End |
| |
| //DebugOut.Analog[22] = MittelIntegralRoll / 26; |
| //DebugOut.Analog[24] = GierGyroFehler; |
| GierGyroFehler = 0; |
| 1097,7 → 1104,7 |
|---|
| // Salvo Ersatzkompass und Giergyrokompensation 15.12.2007 ********************** |
| if ((Kompass_Neuer_Wert > 0)) //nur wenn Kompass einen neuen Wert geliefert hat |
| { |
| Kompass_Neuer_Wert = 0; |
| Kompass_Neuer_Wert = 0; |
| w = (abs(Mittelwert_AccNick)); |
| v = (abs(Mittelwert_AccRoll)); |
| if ((w < ACC_WAAGRECHT_LIMIT) && (v < ACC_WAAGRECHT_LIMIT)) //Ersatzkompass und Giergyro nur mit Magnetkompass aktualisieren wenn alles ok |
| 1108,7 → 1115,7 |
|---|
| if ((abs(StickGier) < 20)) //Giergyroabgleich mit Kompass |
| { |
| if (cnt_stickgier_zero < 255) cnt_stickgier_zero += 1; |
| if (cnt_stickgier_zero > 2) // nur Abgleichen wenn keine Stickbewegung da |
| if (cnt_stickgier_zero > 10) // nur Abgleichen wenn keine Stickbewegung da |
| { |
| w = (int) (GyroGier_Comp/(long)GIER_GRAD_FAKTOR); |
| v = KompassValue - gyrogier_kompass; // realen Drehwinkel seit letztem Ruecksetzen von GyroGier_Comp bestimmen |
| 1166,6 → 1173,8 |
|---|
| } |
| Kompass_Value_Old = KompassValue; |
| } |
| else magkompass_ok = 0; |
| |
| // Salvo End ************************* |
| |
| // Salvo 6.10.2007 |
| 1212,10 → 1221,7 |
|---|
| if(abs(StickGier) > 15) // war 35 |
| { |
| KompassSignalSchlecht = 1000; |
| if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) |
| { |
| NeueKompassRichtungMerken = 1; |
| }; |
| if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
| } |
| tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
| tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
| 1229,8 → 1235,8 |
|---|
| // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
| //DebugOut.Analog[29] = (MaxStickNick + MaxStickRoll); |
| |
| // Salvo 13.9.2007 Nur wenn Magnetkompass ordentliche Werte liefert |
| if ((magkompass_ok > 0) && NeueKompassRichtungMerken) |
| // Salvo 29.12.2008 |
| if (NeueKompassRichtungMerken) |
| { |
| KompassStartwert = KompassValue; |
| NeueKompassRichtungMerken = 0; |
| 1243,7 → 1249,7 |
|---|
| if(w >= 0) |
| { |
| // Salvo Kompasssteuerung ********************** |
| if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
| if (magkompass_ok > 0) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten wenn dieser ok ist |
| // Salvo End |
| } |
| |
| 1292,9 → 1298,13 |
|---|
| DebugOut.Analog[27] = gps_rel_act_position.utm_north; |
| DebugOut.Analog[28] = gps_rel_act_position.utm_alt; |
| DebugOut.Analog[29] = gps_state + (gps_sub_state*10)+(50*gps_cmd); |
| // DebugOut.Analog[29] = magkompass_ok; |
| |
| |
| DebugOut.Analog[30] = GPS_Nick; |
| DebugOut.Analog[31] = GPS_Roll; |
| // DebugOut.Analog[30] = KompassStartwert; |
| // DebugOut.Analog[31] = KompassRichtung; |
| |
| |
| /* DebugOut.Analog[16] = motor_rx[0]; |