Rev 1966 | Go to most recent revision | Show entire file | Ignore whitespace | Details | Blame | Last modification | View Log | RSS feed
Rev 1966 | Rev 1972 | ||
---|---|---|---|
Line 1263... | Line 1263... | ||
1263 | CalculateCompassTimer = 13; // falls keine Navi-Daten |
1263 | CalculateCompassTimer = 13; // falls keine Navi-Daten |
1264 | // max. Korrekturwert schätzen |
1264 | // max. Korrekturwert schätzen |
1265 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1265 | w = abs(IntegralNick /512); // mit zunehmender Neigung den Einfluss drosseln |
1266 | v = abs(IntegralRoll /512); |
1266 | v = abs(IntegralRoll /512); |
1267 | if(v > w) w = v; // grösste Neigung ermitteln |
1267 | if(v > w) w = v; // grösste Neigung ermitteln |
1268 | korrektur = w / 4 + 1; |
1268 | // korrektur = w / 4 + 1; |
- | 1269 | korrektur = w / 8 + 2; |
|
1269 | ErsatzKompassInGrad = ErsatzKompass/GIER_GRAD_FAKTOR; |
1270 | ErsatzKompassInGrad = ErsatzKompass/GIER_GRAD_FAKTOR; |
1270 | // Kompassfehlerwert bestimmen |
1271 | // Kompassfehlerwert bestimmen |
1271 | fehler = ((540 + KompassValue - ErsatzKompassInGrad) % 360) - 180; |
1272 | fehler = ((540 + KompassValue - ErsatzKompassInGrad) % 360) - 180; |
1272 | // GIER_GRAD_FAKTOR ist ca. 1200 |
1273 | // GIER_GRAD_FAKTOR ist ca. 1200 |
1273 | - | ||
1274 | // Kompasswert einloggen |
1274 | // Kompasswert einloggen |
1275 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1275 | if(KompassSignalSchlecht) KompassSignalSchlecht--; |
1276 | else |
1276 | else |
1277 | if(w < 25) |
1277 | if(w < 25) |
1278 | { |
1278 | { |
Line 1296... | Line 1296... | ||
1296 | } |
1296 | } |
1297 | else CompassGierSetpoint = 0; |
1297 | else CompassGierSetpoint = 0; |
1298 | } // CalculateCompassTimer |
1298 | } // CalculateCompassTimer |
1299 | } |
1299 | } |
1300 | else CompassGierSetpoint = 0; |
1300 | else CompassGierSetpoint = 0; |
- | 1301 | ||
- | 1302 | //DebugOut.Analog[16] = KompassFusion; |
|
1301 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1303 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1302 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1304 | // Drehgeschwindigkeit und -winkel zu einem Istwert zusammenfassen |
1303 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1305 | // +++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ |
1304 | if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |
1306 | if(TrichterFlug) { SummeRoll = 0; SummeNick = 0;}; |
Line 1313... | Line 1315... | ||
1313 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
1315 | MesswertNick = IntegralNickMalFaktor + (long)((long)MesswertNick * GyroFaktor + (long)TrimNick * 128L) / (256L / STICK_GAIN); |
1314 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
1316 | MesswertRoll = IntegralRollMalFaktor + (long)((long)MesswertRoll * GyroFaktor + (long)TrimRoll * 128L) / (256L / STICK_GAIN); |
1315 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
1317 | MesswertGier = (long)(MesswertGier * 2 * (long)GyroFaktorGier) / (256L / STICK_GAIN) + (long)(Integral_Gier * IntegralFaktorGier) / (2 * (44000 / STICK_GAIN)); |
Line 1316... | Line 1318... | ||
1316 | 1318 | ||
1317 | // Maximalwerte abfangen |
- | |
1318 | // #define MAX_SENSOR (4096*STICK_GAIN) |
1319 | // Maximalwerte abfangen |
1319 | #define MAX_SENSOR (4096) |
1320 | #define MAX_SENSOR (4096) |
1320 | if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; |
1321 | if(MesswertNick > MAX_SENSOR) MesswertNick = MAX_SENSOR; |
1321 | if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR; |
1322 | if(MesswertNick < -MAX_SENSOR) MesswertNick = -MAX_SENSOR; |
1322 | if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR; |
1323 | if(MesswertRoll > MAX_SENSOR) MesswertRoll = MAX_SENSOR; |
Line 1564... | Line 1565... | ||
1564 | { |
1565 | { |
1565 | SollHoehe = HoehenWert - 400; |
1566 | SollHoehe = HoehenWert - 400; |
1566 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
1567 | if(EE_Parameter.Hoehe_StickNeutralPoint) StickGasHover = EE_Parameter.Hoehe_StickNeutralPoint; |
1567 | else StickGasHover = 120; |
1568 | else StickGasHover = 120; |
1568 | HoverGas = GasMischanteil; |
1569 | HoverGas = GasMischanteil; |
- | 1570 | VarioCharacter = '.'; |
|
1569 | } |
1571 | } |
1570 | HCGas = HoverGas; // take hover gas (neutral point) |
1572 | HCGas = HoverGas; // take hover gas (neutral point) |
1571 | } |
1573 | } |
1572 | if(HoehenWert > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1574 | if(HoehenWert > SollHoehe || !(Parameter_ExtraConfig & CFG2_HEIGHT_LIMIT)) |
1573 | { |
1575 | { |