From 11c897e04f237d26f941989aa9e3a4d8d5c5a7ef Mon Sep 17 00:00:00 2001 From: Lars Toenning Date: Thu, 22 Dec 2022 17:56:34 +0100 Subject: [PATCH] fixup! Add velocity extrapolator --- src/blackcore/airspacemonitor.cpp | 70 +++++++------------ .../simulation/interpolatorvelocity.cpp | 19 ++++- 2 files changed, 43 insertions(+), 46 deletions(-) diff --git a/src/blackcore/airspacemonitor.cpp b/src/blackcore/airspacemonitor.cpp index ec714e5b8..7fa90b5ad 100644 --- a/src/blackcore/airspacemonitor.cpp +++ b/src/blackcore/airspacemonitor.cpp @@ -1272,27 +1272,15 @@ namespace BlackCore // update client info this->autoAdjustCientGndCapability(situation); - // store situation history - Q_ASSERT_X(m_fsdClient, Q_FUNC_INFO, "Empty fsd client"); - const bool isVatsim = m_fsdClient->getServer().getEcosystem().isSystem(CEcosystem::VATSIM); + Q_ASSERT_X(!situation.hasVelocity(), Q_FUNC_INFO, "Velocity of aircraft updates must be invalid"); - if (isVatsim) - { - // We are not using PilotDataUpdate (@) for VATSIM with velocity to update position - // but store this situation to use some data of it with visual pilot updates - m_fullSituations[callsign] = situation; - if (!existsInRange && validMaxRange) - { - // On first recv packet, add position with 0 velocity - CAircraftSituation sit(situation); - sit.setVelocity(CAircraftVelocity()); - this->storeAircraftSituation(sit); - } - } - else - { - this->storeAircraftSituation(situation); // updates situation - } + // store situation history + this->storeAircraftSituation(situation); // updates situation + + // The packet is only stored above, if previously no velocity packet was added. + // To transfer data from the @ update to velocity updates, store this situation separatly + // TODO: Not needed anymore if all packets would be added to the history + m_fullSituations[callsign] = situation; // in case we only have if (!existsInRange && validMaxRange) @@ -1389,28 +1377,23 @@ namespace BlackCore Q_ASSERT_X(situation.isValidVectorRange(), Q_FUNC_INFO, "out of range [-1,1]"); } - // Visual packets do not have groundspeed, hence set the last known value. - // If there is no full position available yet, throw this interim position away. + // Visual packets do not have groundspeed, pressure altitude and onGround flag, + // hence set the last known value from last full update or last visual update. + // If there is no full position available yet, throw this visual position away. CAircraftSituation visualSituation(situation); - - // changed position, continue and copy values - visualSituation.setCurrentUtcTime(); - - // Use data from last full position update (@) if newer or from last stored situation if nothing changed - // If non of the following cases catches (no full update received in the past), use default initialized values - CAircraftSituation lastFullUpdate{}; - auto lastData = m_fullSituations.find(callsign); + CAircraftSituation lastSituation{}; + auto lastDataIt = m_fullSituations.find(callsign); CAircraftSituationList history = this->remoteAircraftSituations(callsign); - if (lastData != m_fullSituations.end()) + if (lastDataIt != m_fullSituations.end()) { // New full update in last data. - lastFullUpdate = *lastData; - m_fullSituations.erase(lastData); + lastSituation = *lastDataIt; + m_fullSituations.erase(lastDataIt); } else if (!history.empty()) { // Use full update info from last position/visual update - lastFullUpdate = history.latestObject(); + lastSituation = history.latestObject(); } else { @@ -1420,21 +1403,20 @@ namespace BlackCore // Throw away visual update return; } - visualSituation.setPressureAltitude(lastFullUpdate.getPressureAltitude()); - visualSituation.setOnGround(lastFullUpdate.isOnGround()); - visualSituation.setGroundSpeed(lastFullUpdate.getGroundSpeed()); + // changed position, continue and copy values + visualSituation.setCurrentUtcTime(); + visualSituation.setPressureAltitude(lastSituation.getPressureAltitude()); + visualSituation.setOnGround(lastSituation.isOnGround()); + visualSituation.setGroundSpeed(lastSituation.getGroundSpeed()); + + Q_ASSERT_X(visualSituation.hasVelocity(), Q_FUNC_INFO, "Velocity of visual updates must be valid"); // store situation history this->storeAircraftSituation(visualSituation); - history = this->remoteAircraftSituations(callsign); - if (!history.empty()) - { - const CAircraftSituation lastSituation = history.latestObject(); - const bool samePosition = lastSituation.equalNormalVectorDouble(visualSituation); - if (samePosition) { return; } // nothing to update - } + const bool samePosition = lastSituation.equalNormalVectorDouble(visualSituation); + if (samePosition) { return; } // nothing to update // update aircraft this->updateAircraftInRangeDistanceBearing( diff --git a/src/blackmisc/simulation/interpolatorvelocity.cpp b/src/blackmisc/simulation/interpolatorvelocity.cpp index e46fbe6fc..b9f03632a 100644 --- a/src/blackmisc/simulation/interpolatorvelocity.cpp +++ b/src/blackmisc/simulation/interpolatorvelocity.cpp @@ -186,7 +186,22 @@ namespace BlackMisc::Simulation CInterpolatorVelocity::CInterpolant CInterpolatorVelocity::getInterpolant(SituationLog &log) { - CAircraftSituation current = *m_currentSituations.begin(); + Q_ASSERT_X(m_currentSituations.size() >= 1, Q_FUNC_INFO, "Velocity interpolator needs at least one situation"); + CAircraftSituation current{}; + // Find first (latest) situation with velocity data + auto it = std::find_if(m_currentSituations.begin(), m_currentSituations.end(), [](const CAircraftSituation &sit){ return sit.hasVelocity(); }); + if (it != m_currentSituations.end()) + { + current = *it; + } + else + { + // No situation with velocity data available. Take the latest full update with velocities set to 0 + current = *m_currentSituations.begin(); + current.setVelocity(CAircraftVelocity(0, 0, 0, CSpeedUnit::m_s(), 0, 0, 0, CAngleUnit::rad(), CTimeUnit::s())); + } + + Q_ASSERT_X(current.hasVelocity(), Q_FUNC_INFO, "Velocity interpolator needs situation with valid velocities"); // adjust ground if required if (!current.canLikelySkipNearGroundInterpolation() && !current.hasGroundElevation()) @@ -194,7 +209,7 @@ namespace BlackMisc::Simulation // The elevation needs to be requested again when we do not receive any velocity updates of an aircraft // for some time and the elevation is therefore discarded from the cache. // This happens when the aircraft is not moving and we only receive a #ST packet once. - const CElevationPlane planeOld = this->findClosestElevationWithinRangeOrRequest(current, CElevationPlane::singlePointRadius(), current.getCallsign()); + const CElevationPlane planeOld = this->findClosestElevationWithinRange(current, CElevationPlane::singlePointRadius()); current.setGroundElevationChecked(planeOld, CAircraftSituation::FromCache); }