855,35 → 855,35 |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
// Kompass |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
int w,v; |
static char SignalSchlecht = 0; |
if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
{ |
int w,v; |
static char SignalSchlecht = 0; |
|
if (!updKompass--) // Aufruf mit ~10 Hz |
if (!updKompass--) // Aufruf mit ~10 Hz |
{ |
updKompass = 50; |
KompassValue = heading_MM3(); |
KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
updKompass = 50; |
|
w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /512); |
if (v > w) w = v; // grösste Neigung ermitteln |
if (w < 40 && NeueKompassRichtungMerken && !SignalSchlecht) |
{ |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if (w > 0) |
{ |
if (!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
else SignalSchlecht--; |
} |
else SignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 1/2 sek |
} |
|
w = abs(IntegralNick /1024); // mit zunehmender Neigung den Einfluss drosseln |
v = abs(IntegralRoll /1024); |
if(v > w) w = v; // grösste Neigung ermitteln |
if(w < 35 && NeueKompassRichtungMerken && !SignalSchlecht) |
{ |
KompassStartwert = KompassValue; |
NeueKompassRichtungMerken = 0; |
} |
w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
if(w > 0) |
{ |
if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
if(SignalSchlecht) SignalSchlecht--; |
} |
else SignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 1/2 sek |
} |
} |
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
|
// +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |