Rev 481 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 481 | Rev 484 | ||
---|---|---|---|
Line 856... | Line 856... | ||
856 | // Kompass |
856 | // Kompass |
857 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
857 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
858 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
858 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
859 | { |
859 | { |
860 | int w,v; |
860 | int w,v; |
861 | static char SignalSchlecht = 0; |
- | |
Line 862... | Line 861... | ||
862 | 861 | ||
863 | if (!updKompass--) // Aufruf mit ~10 Hz |
862 | if (!updKompass--) // Aufruf mit ~10 Hz |
864 | { |
863 | { |
865 | updKompass = 50; |
864 | updKompass = 50; |
866 | KompassValue = heading_MM3(); |
865 | KompassValue = heading_MM3(); |
Line 867... | Line 866... | ||
867 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
866 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
868 | 867 | ||
869 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
868 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
870 | v = abs(IntegralRoll /512); |
869 | v = abs(IntegralRoll /512); |
871 | if (v > w) w = v; // grösste Neigung ermitteln |
870 | if (v > w) w = v; // grösste Neigung ermitteln |
872 | if (w < 40 && NeueKompassRichtungMerken && !SignalSchlecht) |
871 | if (w < 40 && NeueKompassRichtungMerken) |
873 | { |
872 | { |
874 | KompassStartwert = KompassValue; |
873 | KompassStartwert = KompassValue; |
875 | NeueKompassRichtungMerken = 0; |
874 | NeueKompassRichtungMerken = 0; |
876 | } |
875 | } |
877 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
876 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
878 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
877 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
879 | if (w > 0) |
878 | if (w > 0) |
880 | { |
- | |
881 | if (!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
879 | { |
882 | else SignalSchlecht--; |
880 | Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
883 | } |
881 | } |
884 | else SignalSchlecht = 250; // so lange das Signal taub stellen --> ca. 1/2 sek |
882 | else beeptime = 500; |
885 | } |
883 | } |
Line 886... | Line 884... | ||
886 | } |
884 | } |