Rev 718 | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 718 | Rev 725 | ||
---|---|---|---|
Line 73... | Line 73... | ||
73 | volatile long Mess_Integral_Hoch = 0; |
73 | volatile long Mess_Integral_Hoch = 0; |
Line 74... | Line 74... | ||
74 | 74 | ||
75 | int KompassValue = 0; |
75 | int KompassValue = 0; |
76 | int KompassStartwert = 0; |
76 | int KompassStartwert = 0; |
- | 77 | int KompassRichtung = 0; |
|
Line 77... | Line 78... | ||
77 | int KompassRichtung = 0; |
78 | uint8_t updKompass = 0; |
78 | 79 | ||
79 | unsigned char MAX_GAS,MIN_GAS; |
80 | unsigned char MAX_GAS,MIN_GAS; |
80 | unsigned char Notlandung = 0; |
81 | unsigned char Notlandung = 0; |
Line 902... | Line 903... | ||
902 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
903 | //DebugOut.Analog[31] = StickRoll / (26*IntegralFaktor); |
Line 903... | Line 904... | ||
903 | 904 | ||
904 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
905 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
905 | // Gieren |
906 | // Gieren |
906 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
907 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
907 | if(abs(StickGier) > 20) // war 35 |
908 | if(abs(StickGier) > 15) // war 35 |
908 | { |
909 | { |
909 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
910 | if(!(EE_Parameter.GlobalConfig & CFG_KOMPASS_FIX)) NeueKompassRichtungMerken = 1; |
910 | } |
911 | } |
911 | tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
912 | tmp_int = (long)EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
Line 916... | Line 917... | ||
916 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
917 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 917... | Line 918... | ||
917 | 918 | ||
918 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
919 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
919 | // Kompass |
920 | // Kompass |
920 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
921 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
921 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
- | |
922 | { |
- | |
923 | int w,v; |
- | |
924 | static uint8_t updKompass = 0; |
922 | if(EE_Parameter.GlobalConfig & CFG_KOMPASS_AKTIV) |
925 | 923 | { |
|
926 | if (!updKompass--) // Aufruf mit ~10 Hz |
924 | if (!updKompass--) // Aufruf mit ~10 Hz |
927 | { |
925 | { |
928 | KompassValue = heading_MM3(); |
926 | KompassValue = heading_MM3(); |
929 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
927 | KompassRichtung = ((540 + KompassValue - KompassStartwert) % 360) - 180; |
930 | updKompass = 50; |
- | |
931 | } |
- | |
932 | - | ||
933 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
- | |
934 | v = abs(IntegralRoll /512); |
- | |
935 | if(v > w) w = v; // grösste Neigung ermitteln |
928 | updKompass = 50; |
936 | if(w < 35 && NeueKompassRichtungMerken) |
929 | if(NeueKompassRichtungMerken) |
937 | { |
930 | { |
938 | KompassStartwert = KompassValue; |
931 | KompassStartwert = KompassValue; |
939 | NeueKompassRichtungMerken = 0; |
932 | NeueKompassRichtungMerken = 0; |
940 | } |
- | |
941 | w = (w * Parameter_KompassWirkung) / 64; // auf die Wirkung normieren |
- | |
942 | w = Parameter_KompassWirkung - w; // Wirkung ggf drosseln |
- | |
943 | if(w > 0) |
- | |
944 | { |
933 | } |
945 | Mess_Integral_Gier -= (KompassRichtung * w) / 32; // nach Kompass ausrichten |
934 | Mess_Integral_Gier -= (KompassRichtung *Parameter_KompassWirkung); // nach Kompass ausrichten |
946 | } |
935 | } |
Line 947... | Line 936... | ||
947 | } |
936 | } |
948 | 937 | ||
949 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
938 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
950 | // Debugwerte zuordnen |
939 | // Debugwerte zuordnen |
951 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
940 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
952 | if(!TimerWerteausgabe--) |
941 | if(!TimerWerteausgabe--) |
- | 942 | { |
|
953 | { |
943 | TimerWerteausgabe = 24; |
954 | TimerWerteausgabe = 24; |
944 | |
955 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
945 | DebugOut.Analog[0] = IntegralNick / EE_Parameter.GyroAccFaktor; |
956 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
946 | DebugOut.Analog[1] = IntegralRoll / EE_Parameter.GyroAccFaktor; |
957 | DebugOut.Analog[2] = Mittelwert_AccNick; |
947 | DebugOut.Analog[2] = Mittelwert_AccNick; |