flightsimulatorplatfrom/pilotclient
1
0
Fork 0
mirror of https://github.com/swift-project/pilotclient.git synced 2026-03-30 20:15:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

fixup! Add velocity extrapolator

Browse Source 
[skip ci]
This commit is contained in:
Lars Toenning
2022-12-19 23:55:33 +01:00
parent f3700da5aa
commit 6fe2dc8037
2 changed files with 35 additions and 29 deletions
Download Patch File Download Diff File Expand all files Collapse all files

60
src/blackmisc/simulation/interpolatorvelocity.cpp
View File

@@ -49,12 +49,16 @@ namespace BlackMisc::Simulation
m_creationTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch(); m_creationTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch();
m_currentTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch(); m_currentTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch();
// Delta to last shown position (correction vectors) // Get the situation of the aircraft that the old interpolant would show at the current timestamp
// and calculate delta to to this position (correction vectors)
CInterpolant oldCopy(old);
oldCopy.update(currentSituation.getMSecsSinceEpoch());
CAircraftSituation unusedSituation; CAircraftSituation unusedSituation;
CAircraftSituation lastSitutation = old.interpolatePositionAndAltitude(unusedSituation, true); const CAircraftSituation lastSitutation = oldCopy.interpolatePositionAndAltitude(unusedSituation, true);
m_altitudeDelta = lastSitutation.getAltitude().value(CLengthUnit::ft()) - currentSituation.getCorrectedAltitude().value(CLengthUnit::ft());
m_latDelta = lastSitutation.getPosition().latitude().value(CAngleUnit::deg()) - currentSituation.getPosition().latitude().value(CAngleUnit::deg()); m_altitudeDelta = lastSitutation.getAltitude().value(CLengthUnit::m()) - currentSituation.getCorrectedAltitude().value(CLengthUnit::m());
m_lonDelta = lastSitutation.getPosition().longitude().value(CAngleUnit::deg()) - currentSituation.getPosition().longitude().value(CAngleUnit::deg()); m_latDeltaRad = lastSitutation.getPosition().latitude().value(CAngleUnit::rad()) - currentSituation.getPosition().latitude().value(CAngleUnit::rad());
m_lonDeltaRad = lastSitutation.getPosition().longitude().value(CAngleUnit::rad()) - currentSituation.getPosition().longitude().value(CAngleUnit::rad());
auto correctTurnDirection = [](double delta) -> double auto correctTurnDirection = [](double delta) -> double
{ {
@@ -130,40 +134,42 @@ namespace BlackMisc::Simulation
{ {
// Altitude // Altitude
const double currentAltitude = m_currentSitutation.getCorrectedAltitude().value(CLengthUnit::ft()); const double currentAltitude = m_currentSitutation.getCorrectedAltitude().value(CLengthUnit::m());
const double altVelo = m_currentSitutation.getVelocity().getVelocityY(CSpeedUnit::m_s()); const double altVelo = m_currentSitutation.getVelocity().getVelocityY(CSpeedUnit::m_s());
sit.setAltitude(CAltitude(currentAltitude + diffMs/1000.0 * altVelo + errorOffsetFraction * m_altitudeDelta, CLengthUnit::ft())); sit.setAltitude(CAltitude(currentAltitude + diffMs/1000.0 * altVelo + errorOffsetFraction * m_altitudeDelta, CLengthUnit::m()));
} }
{ {
// Position // Position
const CCoordinateGeodetic pos = m_currentSitutation.getPosition(); CCoordinateGeodetic pos = m_currentSitutation.getPosition();
// Latitude // Latitude
const double latVelo = m_currentSitutation.getVelocity().getVelocityZ(CSpeedUnit::m_s()); {
CLength extrapolateLengthLat = CLength(latVelo * diffMs/1000.0, CLengthUnit::m()); const double latVelo = m_currentSitutation.getVelocity().getVelocityZ(CSpeedUnit::m_s());
CCoordinateGeodetic latShifted = pos; const CLength extrapolateLengthLat = CLength(latVelo * diffMs/1000.0, CLengthUnit::m());
// Extrapolate const double shiftDirectionLat = (extrapolateLengthLat.value(CLengthUnit::m()) >= 0 ? 0 : 180);
double degLat = (extrapolateLengthLat.value(CLengthUnit::m()) >= 0 ? 0 : 180); pos = pos.calculatePosition(extrapolateLengthLat.abs(), CAngle(shiftDirectionLat, CAngleUnit::deg()));
latShifted = pos.calculatePosition(extrapolateLengthLat.abs(), CAngle(degLat, CAngleUnit::deg())); }
// Account delta since last update (error)
latShifted.setLatitude(latShifted.latitude() + CLatitude(errorOffsetFraction * m_latDelta, CAngleUnit::deg()));
// Longitude // Longitude
const double lonVelo = m_currentSitutation.getVelocity().getVelocityX(CSpeedUnit::m_s()); {
CLength extrapolateLengthLon = CLength(lonVelo * diffMs/1000.0, CLengthUnit::m()); const double lonVelo = m_currentSitutation.getVelocity().getVelocityX(CSpeedUnit::m_s());
CCoordinateGeodetic lonShifted = pos; const CLength extrapolateLengthLon = CLength(lonVelo * diffMs/1000.0, CLengthUnit::m());
// Extrapolate const double shiftDirectionLon = (extrapolateLengthLon.value(CLengthUnit::m()) >= 0 ? 90 : 270);
double degLon = (extrapolateLengthLon.value(CLengthUnit::m()) >= 0 ? 90 : 270); pos = pos.calculatePosition(extrapolateLengthLon.abs(), CAngle(shiftDirectionLon, CAngleUnit::deg()));
lonShifted = pos.calculatePosition(extrapolateLengthLon.abs(), CAngle(degLon, CAngleUnit::deg())); }
// Account delta since last update (error)
lonShifted.setLongitude(lonShifted.longitude() + CLongitude(errorOffsetFraction * m_lonDelta, CAngleUnit::deg()));
CCoordinateGeodetic combined = sit.getPosition(); // Apply error
combined.setLatLong(latShifted.latitude(), lonShifted.longitude()); const CLatitude latError(errorOffsetFraction * m_latDeltaRad, CAngleUnit::rad());
pos.setLatitude(pos.latitude() + latError);
const CLongitude lonError(errorOffsetFraction * m_lonDeltaRad, CAngleUnit::rad());
pos.setLongitude(pos.longitude() + lonError);
sit.setPosition(combined); // Copy altitude to avoid overriding with new position
const CAltitude altitude = sit.getAltitude();
sit.setPosition(pos);
sit.setAltitude(altitude);
} }
// Set ground factor directly from update (no extra/interpolation for now) // Set ground factor directly from update (no extra/interpolation for now)

4
src/blackmisc/simulation/interpolatorvelocity.h
View File

@@ -65,8 +65,8 @@ namespace BlackMisc
qint64 m_creationTimeMsSinceEpoch; qint64 m_creationTimeMsSinceEpoch;
qint64 m_currentTimeMsSinceEpoch; qint64 m_currentTimeMsSinceEpoch;
double m_altitudeDelta = 0; double m_altitudeDelta = 0;
double m_latDelta = 0; double m_latDeltaRad = 0;
double m_lonDelta = 0; double m_lonDeltaRad = 0;
double m_bankDelta = 0; double m_bankDelta = 0;
double m_headingDelta = 0; double m_headingDelta = 0;
double m_pitchDelta = 0; double m_pitchDelta = 0;