Rev 1943 | Rev 1945 | Go to most recent revision | Show entire file | Regard whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1943 | Rev 1944 | ||
---|---|---|---|
Line 1041... | Line 1041... | ||
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); |
1045 | tmp_long2 = (tmp_long2 * FromNaviCtrl_Value.Kalman_K) / (32 * 16); |
1046 | KompassFusion = FromNaviCtrl_Value.Kalman_K; |
- | |
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 | } |
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; |