Rev 1943 | Rev 1945 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1943 | Rev 1944 | ||
---|---|---|---|
Line 1037... | Line 1037... | ||
1037 | //DebugOut.Analog[18] = (Mittelwert_AccRoll-FromNaviCtrl.AccErrorR) / 4; |
1037 | //DebugOut.Analog[18] = (Mittelwert_AccRoll-FromNaviCtrl.AccErrorR) / 4; |
1038 | //DebugOut.Analog[19] = -FromNaviCtrl.AccErrorR / 4; |
1038 | //DebugOut.Analog[19] = -FromNaviCtrl.AccErrorR / 4; |
Line 1039... | Line 1039... | ||
1039 | 1039 | ||
1040 | if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/) |
1040 | if(FromNaviCtrl_Value.Kalman_K > 0 /*&& !TrichterFlug*/) |
1041 | { |
1041 | { |
1042 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN)); |
1042 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccNick - FromNaviCtrl.AccErrorN)); |
1043 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR)); |
1043 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)(Mittelwert_AccRoll - FromNaviCtrl.AccErrorR)); |
1044 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1044 | tmp_long = (tmp_long * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1045 | tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
- | |
1046 | KompassFusion = FromNaviCtrl_Value.Kalman_K; |
1045 | tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1047 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
1046 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
1048 | { |
1047 | { |
1049 | tmp_long /= 2; |
1048 | tmp_long /= 2; |
1050 | tmp_long2 /= 2; |
1049 | tmp_long2 /= 2; |
1051 | } |
1050 | } |
1052 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
1051 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
1053 | { |
1052 | { |
1054 | tmp_long /= 3; |
1053 | tmp_long /= 3; |
1055 | tmp_long2 /= 3; |
1054 | tmp_long2 /= 3; |
1056 | } |
1055 | } |
1057 | if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
1056 | if(tmp_long > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
1058 | if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
1057 | if(tmp_long < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
1059 | if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
1058 | if(tmp_long2 > (long) FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long) FromNaviCtrl_Value.Kalman_MaxFusion; |
1060 | if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
1059 | if(tmp_long2 < (long)-FromNaviCtrl_Value.Kalman_MaxFusion) tmp_long2 = (long)-FromNaviCtrl_Value.Kalman_MaxFusion; |
1061 | } |
1060 | } |
1062 | else |
1061 | else |
1063 | { |
1062 | { |
1064 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
1063 | tmp_long = (long)(IntegralNick / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccNick); |
1065 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
1064 | tmp_long2 = (long)(IntegralRoll / EE_Parameter.GyroAccFaktor - (long)Mittelwert_AccRoll); |
1066 | tmp_long /= 16; |
1065 | tmp_long /= 16; |
1067 | tmp_long2 /= 16; |
1066 | tmp_long2 /= 16; |
1068 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
1067 | if((MaxStickNick > 64) || (MaxStickRoll > 64)) |
1069 | { |
1068 | { |
1070 | tmp_long /= 3; |
1069 | tmp_long /= 3; |
1071 | tmp_long2 /= 3; |
1070 | tmp_long2 /= 3; |
1072 | } |
1071 | } |
1073 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
1072 | if(abs(PPM_in[EE_Parameter.Kanalbelegung[K_GIER]]) > 25) |
1074 | { |
1073 | { |
1075 | tmp_long /= 3; |
1074 | tmp_long /= 3; |
1076 | tmp_long2 /= 3; |
1075 | tmp_long2 /= 3; |
1077 | } |
1076 | } |
1078 | KompassFusion = 25; |
1077 | KompassFusion = 25; |
1079 | #define AUSGLEICH 32 |
1078 | #define AUSGLEICH 32 |
1080 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
1079 | if(tmp_long > AUSGLEICH) tmp_long = AUSGLEICH; |
1081 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
1080 | if(tmp_long < -AUSGLEICH) tmp_long =-AUSGLEICH; |
1082 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
1081 | if(tmp_long2 > AUSGLEICH) tmp_long2 = AUSGLEICH; |
1083 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
1082 | if(tmp_long2 <-AUSGLEICH) tmp_long2 =-AUSGLEICH; |
Line 1084... | Line 1083... | ||
1084 | } |
1083 | } |
1085 | 1084 | ||
1086 | Mess_IntegralNick -= tmp_long; |
1085 | Mess_IntegralNick -= tmp_long; |
Line 1246... | Line 1245... | ||
1246 | if(!(Parameter_GlobalConfig & CFG_KOMPASS_FIX)) |
1245 | if(!(Parameter_GlobalConfig & CFG_KOMPASS_FIX)) |
1247 | { |
1246 | { |
1248 | NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen |
1247 | NeueKompassRichtungMerken = 50; // eine Sekunde zum Einloggen |
1249 | }; |
1248 | }; |
1250 | } |
1249 | } |
1251 | tmp_int = (long) EE_Parameter.Gier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1250 | tmp_int = (long) EE_Parameter.StickGier_P * ((long)StickGier * abs(StickGier)) / 512L; // expo y = ax + bx² |
1252 | tmp_int += (EE_Parameter.Gier_P * StickGier) / 4; |
1251 | tmp_int += (EE_Parameter.StickGier_P * StickGier) / 4; |
1253 | tmp_int += CompassGierSetpoint; |
1252 | tmp_int += CompassGierSetpoint; |
1254 | sollGier = tmp_int; |
1253 | sollGier = tmp_int; |
1255 | Mess_Integral_Gier -= tmp_int; |
1254 | Mess_Integral_Gier -= tmp_int; |
1256 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
1255 | if(Mess_Integral_Gier > 50000) Mess_Integral_Gier = 50000; // begrenzen |
1257 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
1256 | if(Mess_Integral_Gier <-50000) Mess_Integral_Gier =-50000; |
Line 1283... | Line 1282... | ||
1283 | GierGyroFehler += fehler; |
1282 | GierGyroFehler += fehler; |
1284 | if(NeueKompassRichtungMerken) |
1283 | if(NeueKompassRichtungMerken) |
1285 | { |
1284 | { |
1286 | if(--NeueKompassRichtungMerken == 0) |
1285 | if(--NeueKompassRichtungMerken == 0) |
1287 | { |
1286 | { |
1288 | //--> ErsatzKompass = KompassValue * GIER_GRAD_FAKTOR; |
- | |
1289 | KompassSollWert = ErsatzKompassInGrad; |
1287 | KompassSollWert = ErsatzKompassInGrad; |
1290 | } |
1288 | } |
1291 | } |
1289 | } |
1292 | } |
1290 | } |
1293 | // Kompass fusionieren |
1291 | // Kompass fusionieren |
1294 | if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur; |
1292 | if(!KompassSignalSchlecht) ErsatzKompass += (fehler * KompassFusion) / korrektur; |
1295 | - | ||
- | 1293 | DebugOut.Analog[16] = KompassFusion; |
|
1296 | // MK Gieren |
1294 | // MK Gieren |
1297 | if(!NeueKompassRichtungMerken) |
1295 | if(!NeueKompassRichtungMerken) |
1298 | { |
1296 | { |
1299 | r = ((540 + (KompassSollWert - ErsatzKompassInGrad)) % 360) - 180; |
1297 | r = ((540 + (KompassSollWert - ErsatzKompassInGrad)) % 360) - 180; |
1300 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
1298 | v = r * (Parameter_KompassWirkung/2); // nach Kompass ausrichten |
Line 1726... | Line 1724... | ||
1726 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
1724 | if(GasMischanteil > (MAX_GAS - 20) * STICK_GAIN) GasMischanteil = (MAX_GAS - 20) * STICK_GAIN; |
Line 1727... | Line 1725... | ||
1727 | 1725 | ||
1728 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1726 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1729 | // all BL-Ctrl connected? |
1727 | // all BL-Ctrl connected? |
1730 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1728 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1731 | if(MissingMotor || Capacity.MinOfMaxPWM != 255) // wait until all BL-Ctrls started |
1729 | if(MissingMotor || Capacity.MinOfMaxPWM != 255 || NC_ErrorCode) // wait until all BL-Ctrls started and no Errors |
1732 | if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) // only during start-phase |
1730 | if(modell_fliegt > 1 && modell_fliegt < 50 && GasMischanteil > 0) // only during start-phase |
1733 | { |
1731 | { |
1734 | modell_fliegt = 1; |
1732 | modell_fliegt = 1; |
1735 | GasMischanteil = (MIN_GAS + 10) * STICK_GAIN; |
1733 | GasMischanteil = (MIN_GAS + 10) * STICK_GAIN; |