Rev 718 | Go to most recent revision | 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; |