Rev 1966 | Rev 1982 | 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 | { |