Rev 690 | Rev 700 | 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 | ||