Rev 690 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 690 | Rev 694 | ||
---|---|---|---|
Line 62... | Line 62... | ||
62 | #include "fc.h" |
62 | #include "fc.h" |
63 | #include "gps.h" |
63 | #include "gps.h" |
64 | #include "uart.h" |
64 | #include "uart.h" |
65 | #include "rc.h" |
65 | #include "rc.h" |
66 | #include "twimaster.h" |
66 | #include "twimaster.h" |
- | 67 | #include "mm3.h" |
|
Line 67... | Line 68... | ||
67 | 68 | ||
68 | unsigned char h,m,s; |
69 | unsigned char h,m,s; |
69 | volatile unsigned int I2CTimeout = 100; |
70 | volatile unsigned int I2CTimeout = 100; |
70 | volatile int MesswertNick,MesswertRoll,MesswertGier; |
71 | volatile int MesswertNick,MesswertRoll,MesswertGier; |
Line 929... | Line 930... | ||
929 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
930 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 930... | Line 931... | ||
930 | 931 | ||
931 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
932 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
932 | // Kompass |
933 | // Kompass |
933 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
934 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
934 | if(KompassValue && (ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE)) |
935 | if(ParamSet.GlobalConfig & CFG_COMPASS_ACTIVE) |
935 | { |
936 | { |
936 | int w,v; |
937 | int w,v; |
- | 938 | static uint8_t updKompass = 0; |
|
- | 939 | ||
- | 940 | if (!updKompass--) // Aufruf mit ~10 Hz |
|
- | 941 | { |
|
- | 942 | KompassValue = MM3_heading(); |
|
- | 943 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
|
- | 944 | updKompass = 50; |
|
- | 945 | } |
|
937 | static int SignalSchlecht = 0; |
946 | |
938 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
947 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
939 | v = abs(IntegralRoll /512); |
948 | v = abs(IntegralRoll /512); |
940 | if(v > w) w = v; // grösste Neigung ermitteln |
949 | if(v > w) w = v; // grösste Neigung ermitteln |
941 | if(w < 25 && NeueKompassRichtungMerken && !SignalSchlecht) |
950 | if(w < 35 && NeueKompassRichtungMerken) |
942 | { |
951 | { |
943 | KompassStartwert = KompassValue; |
952 | KompassStartwert = KompassValue; |
944 | NeueKompassRichtungMerken = 0; |
953 | NeueKompassRichtungMerken = 0; |
945 | } |
954 | } |
946 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
955 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
947 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
956 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
948 | if(w > 0) |
957 | if(w > 0) |
949 | { |
958 | { |
950 | if(!SignalSchlecht) Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
- | |
951 | if(SignalSchlecht) SignalSchlecht--; |
959 | Mess_Integral_Gier += (KompassRichtung * w) / 32; // nach Kompass ausrichten |
952 | } |
- | |
953 | else SignalSchlecht = 500; // so lange das Signal taub stellen --> ca. 1 sek |
960 | } |
954 | } |
961 | } |
Line 955... | Line 962... | ||
955 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
962 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
956 | 963 |