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