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