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; |