From 48d5e0daec805ae899fed8f3799f9fe762a8b17d Mon Sep 17 00:00:00 2001 From: Klaus Basan Date: Thu, 18 Oct 2018 17:48:54 +0200 Subject: [PATCH] Ref T397, interpolation / interpolant fixes * fixed fillSituationsArray to return correct initial situations * VERIFY ranges and times to avoid inf/NaN and issues in general * if invalid situations, continue with last situation * count invalid situations --- src/blackmisc/simulation/interpolant.h | 7 ++ src/blackmisc/simulation/interpolator.cpp | 109 ++++++++++-------- src/blackmisc/simulation/interpolator.h | 5 + .../simulation/interpolatorspline.cpp | 86 +++++++++----- src/blackmisc/simulation/interpolatorspline.h | 3 + 5 files changed, 138 insertions(+), 72 deletions(-) diff --git a/src/blackmisc/simulation/interpolant.h b/src/blackmisc/simulation/interpolant.h index 1c81adfa0..aa0235ad3 100644 --- a/src/blackmisc/simulation/interpolant.h +++ b/src/blackmisc/simulation/interpolant.h @@ -31,6 +31,12 @@ namespace BlackMisc //! Situations available int getSituationsAvailable() const { return m_situationsAvailable; } + //! Valid? + bool isValid() const { return m_valid; } + + //! Valid? + void setValid(bool valid) { m_valid = valid; } + protected: //! Default ctor IInterpolant() {} @@ -44,6 +50,7 @@ namespace BlackMisc qint64 m_interpolatedTime = -1; //!< "Real time "of interpolated situation int m_situationsAvailable = 0; //!< used situations CInterpolatorPbh m_pbh; //!< the used PBH interpolator + bool m_valid = true; //!< valid? }; } // namespace } // namespace diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index e72fe626c..7c0994b81 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -130,7 +130,7 @@ namespace BlackMisc bool details = false; if (situations.containsOnGroundDetails(CAircraftSituation::InFromParts)) { - // if a client supports parts, all situations are supposed to be parts based + // if a client supports parts, all ground situations are supposed to be parts based details = situations.areAllOnGroundDetailsSame(CAircraftSituation::InFromParts); BLACK_VERIFY_X(details, Q_FUNC_INFO, "Once gnd.from parts -> always gnd. from parts"); } @@ -182,60 +182,77 @@ namespace BlackMisc // CInterpolatorLinear::Interpolant or CInterpolatorSpline::Interpolant SituationLog log; const auto interpolant = derived()->getInterpolant(log); - const CInterpolatorPbh pbh = interpolant.pbh(); + const bool isValid = interpolant.isValid(); - // init interpolated situation - CAircraftSituation currentSituation = this->initInterpolatedSituation(pbh.getOldSituation(), pbh.getNewSituation()); - - // Pitch bank heading first, so follow up steps could use those values - currentSituation.setHeading(pbh.getHeading()); - currentSituation.setPitch(pbh.getPitch()); - currentSituation.setBank(pbh.getBank()); - currentSituation.setGroundSpeed(pbh.getGroundSpeed()); - - // use derived interpolant function - const bool interpolateGndFlag = pbh.getNewSituation().hasGroundDetailsForGndInterpolation() && pbh.getOldSituation().hasGroundDetailsForGndInterpolation(); - currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation, interpolateGndFlag); - if (CBuildConfig::isLocalDeveloperDebugBuild()) - { - Q_ASSERT_X(currentSituation.isValidVectorRange(), Q_FUNC_INFO, "Invalid interpolation situation"); - } - if (!interpolateGndFlag) { currentSituation.guessOnGround(CAircraftSituationChange::null(), m_model); } - - // as we now have the position and can interpolate elevation - currentSituation.interpolateElevation(pbh.getOldSituation(), pbh.getNewSituation()); - if (!currentSituation.hasGroundElevation()) - { - // we still have no elevation - const CLength radius = currentSituation.getDistancePerTime250ms(CElevationPlane::singlePointRadius()); - if (!m_lastSituation.transferGroundElevationFromThis(currentSituation, radius)) - { - const CElevationPlane groundElevation = this->findClosestElevationWithinRange(currentSituation, radius); - m_lastSituation.setGroundElevationChecked(groundElevation, CAircraftSituation::FromCache); - } - } - - // correct altitude itself + CAircraftSituation currentSituation = m_lastSituation; CAircraftSituation::AltitudeCorrection altCorrection = CAircraftSituation::NoCorrection; - if (!interpolateGndFlag && currentSituation.getOnGroundDetails() != CAircraftSituation::OnGroundByGuessing) - { - // just in case - altCorrection = currentSituation.correctAltitude(true); // we have CG set - } - // correct pitch on ground - if (currentSituation.isOnGround()) + if (isValid) { - const CAngle correctedPitchOnGround = m_currentSetup.getPitchOnGround(); - if (!correctedPitchOnGround.isNull()) + const CInterpolatorPbh pbh = interpolant.pbh(); + + // init interpolated situation + currentSituation = this->initInterpolatedSituation(pbh.getOldSituation(), pbh.getNewSituation()); + + // Pitch bank heading first, so follow up steps could use those values + currentSituation.setHeading(pbh.getHeading()); + currentSituation.setPitch(pbh.getPitch()); + currentSituation.setBank(pbh.getBank()); + currentSituation.setGroundSpeed(pbh.getGroundSpeed()); + + // use derived interpolant function + const bool interpolateGndFlag = pbh.getNewSituation().hasGroundDetailsForGndInterpolation() && pbh.getOldSituation().hasGroundDetailsForGndInterpolation(); + currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation, interpolateGndFlag); + if (CBuildConfig::isLocalDeveloperDebugBuild()) { - currentSituation.setPitch(correctedPitchOnGround); + Q_ASSERT_X(currentSituation.isValidVectorRange(), Q_FUNC_INFO, "Invalid interpolation situation"); + } + if (!interpolateGndFlag) { currentSituation.guessOnGround(CAircraftSituationChange::null(), m_model); } + + // as we now have the position and can interpolate elevation + currentSituation.interpolateElevation(pbh.getOldSituation(), pbh.getNewSituation()); + if (!currentSituation.hasGroundElevation()) + { + // we still have no elevation + const CLength radius = currentSituation.getDistancePerTime250ms(CElevationPlane::singlePointRadius()); + if (!m_lastSituation.transferGroundElevationFromThis(currentSituation, radius)) + { + const CElevationPlane groundElevation = this->findClosestElevationWithinRange(currentSituation, radius); + m_lastSituation.setGroundElevationChecked(groundElevation, CAircraftSituation::FromCache); + } + } + + // correct altitude itself + if (!interpolateGndFlag && currentSituation.getOnGroundDetails() != CAircraftSituation::OnGroundByGuessing) + { + // just in case + altCorrection = currentSituation.correctAltitude(true); // we have CG set + } + + // correct pitch on ground + if (currentSituation.isOnGround()) + { + const CAngle correctedPitchOnGround = m_currentSetup.getPitchOnGround(); + if (!correctedPitchOnGround.isNull()) + { + currentSituation.setPitch(correctedPitchOnGround); + } } } + else + { + m_invalidSituations++; + // further handling could go here, mainly we continue with last situation + + if (m_invalidSituations < 3 || (m_invalidSituations % 10) == 0) + { + CLogMessage(this).warning("Invalid situation no %1 for interpolation reported for '%2'") << m_invalidSituations << m_callsign.asString(); + } + }// valid? // status Q_ASSERT_X(currentSituation.hasMSLGeodeticHeight(), Q_FUNC_INFO, "No MSL altitude"); - m_currentInterpolationStatus.setInterpolatedAndCheckSituation(true, currentSituation); + m_currentInterpolationStatus.setInterpolatedAndCheckSituation(isValid, currentSituation); m_lastSituation = currentSituation; Q_ASSERT_X(m_currentInterpolationStatus.hasValidInterpolatedSituation(), Q_FUNC_INFO, "Expect valid situation"); @@ -252,6 +269,8 @@ namespace BlackMisc log.elevationInfo = this->getElevationsFoundMissedInfo(); log.cgAboveGround = currentSituation.getCG(); log.sceneryOffset = m_currentSceneryOffset; + log.noInvalidSituations = m_invalidSituations; + log.noNetworkSituations = m_currentSituations.sizeInt(); m_logger->logInterpolation(log); } diff --git a/src/blackmisc/simulation/interpolator.h b/src/blackmisc/simulation/interpolator.h index 452f87ea8..b17aaf7f3 100644 --- a/src/blackmisc/simulation/interpolator.h +++ b/src/blackmisc/simulation/interpolator.h @@ -220,6 +220,7 @@ namespace BlackMisc //! Clear all data //! \remark mainly needed in UNIT tests + //! \private void clear(); //! Init, or re-init the corressponding model @@ -229,6 +230,9 @@ namespace BlackMisc //! Mark as unit test void markAsUnitTest(); + //! Get count of invalid situations + int getInvalidSituationsCount() const { return m_invalidSituations; } + protected: //! Constructor CInterpolator(const Aviation::CCallsign &callsign, @@ -276,6 +280,7 @@ namespace BlackMisc CPartsStatus m_lastPartsStatus; //!< status for last parts, used when last parts are re-used because of m_partsToSituationInterpolationRatio int m_partsToSituationInterpolationRatio = 2; //!< ratio between parts and situation interpolation, 1..always, 2..every 2nd situation int m_partsToSituationGuessingRatio = 5; //!< ratio between parts guessing and situation interpolation + int m_invalidSituations = 0; //!< mainly when there are no new situations Aviation::CAircraftSituation m_lastSituation { Aviation::CAircraftSituation::null() }; //!< latest interpolation Aviation::CAircraftParts m_lastParts { Aviation::CAircraftParts::null() }; //!< latest parts PhysicalQuantities::CLength m_currentSceneryOffset { PhysicalQuantities::CLength::null() }; //!< calculated scenery offset if any diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index d95002c2f..5e0b0d06d 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -95,6 +95,13 @@ namespace BlackMisc const double a = k0 * (x1 - x0) - (y1 - y0); const double b = -k1 * (x1 - x0) + (y1 - y0); const double y = (1 - t) * y0 + t * y1 + t * (1 - t) * (a * (1 - t) + b * t); + + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + BLACK_VERIFY_X(t >= 0, Q_FUNC_INFO, "Expect t >= 0"); + BLACK_VERIFY_X(t <= 1.0, Q_FUNC_INFO, "Expect t <= 1"); + + } return y; } } @@ -104,35 +111,44 @@ namespace BlackMisc // m_s[0] .. oldest -> m_[2] .. latest // general idea, we interpolate from current situation -> latest situation + // do we have the last interpolated situation? if (m_lastSituation.isNull()) { if (!m_currentSituations.isEmpty()) { + // we start with the latest situation just to init the values m_s[0] = m_s[1] = m_s[2] = m_currentSituations.front(); - m_s[0].addMsecs(-CFsdSetup::c_positionTimeOffsetMsec * 2); // Ref T297 default offset time to fill data - m_s[1].addMsecs(-CFsdSetup::c_positionTimeOffsetMsec); // Ref T297 default offset time to fill data - return true; + const qint64 os = qMin(CFsdSetup::c_interimPositionTimeOffsetMsec, m_s[2].getTimeOffsetMs()); + m_s[0].addMsecs(m_currentTimeMsSinceEpoch - os); // Ref T297 default offset time to fill data + m_s[1].addMsecs(m_currentTimeMsSinceEpoch); // Ref T297 default offset time to fill data + const bool valid = m_s[2].getAdjustedMSecsSinceEpoch() > m_currentTimeMsSinceEpoch; + return valid; } m_s[0] = m_s[1] = m_s[2] = CAircraftSituation::null(); return false; } else { - m_s[0] = m_s[1] = m_s[2] = m_lastSituation; // current - m_s[0].addMsecs(-CFsdSetup::c_positionTimeOffsetMsec); // oldest, Ref T297 default offset time to fill data - m_s[2].addMsecs(CFsdSetup::c_positionTimeOffsetMsec); // latest, Ref T297 default offset time to fill data - if (m_currentSituations.isEmpty()) { return true; } + // in normal cases init some default values + m_s[0] = m_s[1] = m_s[2] = m_lastSituation; // current position + const qint64 os = qMin(CFsdSetup::c_interimPositionTimeOffsetMsec, m_s[2].getTimeOffsetMs()); + m_s[1].setMSecsSinceEpoch(m_currentTimeMsSinceEpoch); + m_s[0].addMsecs(-os); // oldest, Ref T297 default offset time to fill data + m_s[2].addMsecs(os); // latest, Ref T297 default offset time to fill data + if (m_currentSituations.isEmpty()) { return false; } } - const qint64 currentAdjusted = m_s[1].getAdjustedMSecsSinceEpoch(); const CAircraftSituation latest = m_currentSituations.front(); if (latest.isNewerThanAdjusted(m_s[1])) { m_s[2] = latest; } + const qint64 currentAdjusted = m_s[1].getAdjustedMSecsSinceEpoch(); const CAircraftSituation older = m_currentSituations.findObjectBeforeAdjustedOrDefault(currentAdjusted); if (!older.isNull()) { m_s[0] = older; } + const bool valid = m_s[2].getAdjustedMSecsSinceEpoch() > m_currentTimeMsSinceEpoch; if (CBuildConfig::isLocalDeveloperDebugBuild()) { - const bool verified = this->verifyInterpolationSituations(m_s[0], m_s[1], m_s[2]); // oldest -> latest, only verify order + BLACK_VERIFY_X(m_s[2].getAdjustedMSecsSinceEpoch() > m_currentTimeMsSinceEpoch, Q_FUNC_INFO, "No new situation"); + const bool verified = valid && this->verifyInterpolationSituations(m_s[0], m_s[1], m_s[2]); // oldest -> latest, only verify order if (!verified) { static const QString vm("m0-2 (oldest latest) %1 %2 %3"); @@ -140,7 +156,7 @@ namespace BlackMisc Q_UNUSED(vmValues); } } - return true; + return valid; } // pin vtables to this file @@ -150,7 +166,9 @@ namespace BlackMisc CInterpolatorSpline::CInterpolant CInterpolatorSpline::getInterpolant(SituationLog &log) { // recalculate derivatives only if they changed - const bool recalculate = m_situationsLastModified > m_situationsLastModifiedUsed; + const bool recalculate = (m_currentTimeMsSinceEpoch >= m_nextSampleAdjustedTime) || // new step + (m_situationsLastModified > m_situationsLastModifiedUsed); // modified + if (recalculate) { // with the latest updates of T243 the order and the offsets are supposed to be correct @@ -159,13 +177,15 @@ namespace BlackMisc const bool fillStatus = this->fillSituationsArray(); if (!fillStatus) { + m_interpolant.setValid(false); return m_interpolant; } + const std::array, 3> normals {{ m_s[0].getPosition().normalVectorDouble(), m_s[1].getPosition().normalVectorDouble(), m_s[2].getPosition().normalVectorDouble() }}; PosArray pa; pa.x = {{ normals[0][0], normals[1][0], normals[2][0] }}; // oldest -> latest pa.y = {{ normals[0][1], normals[1][1], normals[2][1] }}; - pa.z = {{ normals[0][2], normals[1][2], normals[2][2] }}; + pa.z = {{ normals[0][2], normals[1][2], normals[2][2] }}; // latest pa.t = {{ static_cast(m_s[0].getAdjustedMSecsSinceEpoch()), static_cast(m_s[1].getAdjustedMSecsSinceEpoch()), static_cast(m_s[2].getAdjustedMSecsSinceEpoch()) }}; pa.dx = getDerivatives(pa.t, pa.x); @@ -207,14 +227,20 @@ namespace BlackMisc // cur.time 6: dt1=6-5=1, dt2=7-5 => fraction 1/2 // cur.time 9: dt1=9-7=2, dt2=10-7=3 => fraction 2/3 // we use different offset times for fast pos. updates + // KB: is that correct with dt2, or would it be m_nextSampleTime - m_prevSampleTime + // as long as the offset time is constant, it does not matter const double dt1 = static_cast(m_currentTimeMsSinceEpoch - m_prevSampleAdjustedTime); - const double dt2 = static_cast(m_nextSampleAdjustedTime - m_prevSampleAdjustedTime); + const double dt2 = static_cast(m_nextSampleAdjustedTime - m_prevSampleAdjustedTime); const double timeFraction = dt1 / dt2; - // is that correct with dt2, or would it be - // m_nextSampleTime - m_prevSampleTime - // as long as the offset time is constant, it does not matter - const qint64 interpolatedTime = m_prevSampleTime + timeFraction * dt2; + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + BLACK_VERIFY_X(dt1 >= 0, Q_FUNC_INFO, "Expect postive dt1"); + BLACK_VERIFY_X(dt2 > 0, Q_FUNC_INFO, "Expect postive dt2"); + BLACK_VERIFY_X(timeFraction < 1.01, Q_FUNC_INFO, "Expect fraction 0-1"); + } + + const qint64 interpolatedTime = m_prevSampleTime + qRound(timeFraction * dt2); // time fraction is expected between 0-1 m_currentInterpolationStatus.setInterpolated(true); @@ -226,7 +252,6 @@ namespace BlackMisc log.interpolationSituations.push_back(m_s[0]); log.interpolationSituations.push_back(m_s[1]); log.interpolationSituations.push_back(m_s[2]); // latest at end - log.noNetworkSituations = m_currentSituations.size(); log.interpolator = 's'; log.deltaSampleTimesMs = dt2; log.simTimeFraction = timeFraction; @@ -277,21 +302,33 @@ namespace BlackMisc CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation, bool interpolateGndFactor) const { const double t1 = m_pa.t[1]; - const double t2 = m_pa.t[2]; + const double t2 = m_pa.t[2]; // latest (adjusted) + + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + Q_ASSERT_X(t1 < t2, Q_FUNC_INFO, "Expect sorted times, latest first"); + Q_ASSERT_X(m_currentTimeMsSinceEpoc >= t1, Q_FUNC_INFO, "invalid timestamp t1"); + Q_ASSERT_X(m_currentTimeMsSinceEpoc < t2, Q_FUNC_INFO, "invalid timestamp t2"); // t1==t2 results in div/0 + } + const double newX = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.x[1], m_pa.x[2], m_pa.dx[1], m_pa.dx[2]); const double newY = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.y[1], m_pa.y[2], m_pa.dy[1], m_pa.dy[2]); const double newZ = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.z[1], m_pa.z[2], m_pa.dz[1], m_pa.dz[2]); if (CBuildConfig::isLocalDeveloperDebugBuild()) { - Q_ASSERT_X(CAircraftSituation::isValidVector(m_pa.x), Q_FUNC_INFO, "invalid X"); // all x values - Q_ASSERT_X(CAircraftSituation::isValidVector(m_pa.y), Q_FUNC_INFO, "invalid Y"); // all y values - Q_ASSERT_X(CAircraftSituation::isValidVector(m_pa.z), Q_FUNC_INFO, "invalid Z"); // all z values + BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.x), Q_FUNC_INFO, "invalid X"); // all x values + BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.y), Q_FUNC_INFO, "invalid Y"); // all y values + BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.z), Q_FUNC_INFO, "invalid Z"); // all z values } CAircraftSituation newSituation(currentSituation); const std::array normalVector = {{ newX, newY, newZ }}; const CCoordinateGeodetic currentPosition(normalVector); + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + BLACK_VERIFY_X(CAircraftSituation::isValidVector(normalVector), Q_FUNC_INFO, "invalid vector"); + } const double newA = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.a[1], m_pa.a[2], m_pa.da[1], m_pa.da[2]); const CAltitude alt(newA, m_altitudeUnit); @@ -299,11 +336,6 @@ namespace BlackMisc newSituation.setPosition(currentPosition); newSituation.setAltitude(alt); newSituation.setMSecsSinceEpoch(this->getInterpolatedTime()); - if (CBuildConfig::isLocalDeveloperDebugBuild()) - { - Q_ASSERT_X(CAircraftSituation::isValidVector(normalVector), Q_FUNC_INFO, "invalid vector"); - Q_ASSERT_X(newSituation.isValidVectorRange(), Q_FUNC_INFO, "invalid situation"); - } if (interpolateGndFactor) { diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h index 0e0e7935c..ce6e915f3 100644 --- a/src/blackmisc/simulation/interpolatorspline.h +++ b/src/blackmisc/simulation/interpolatorspline.h @@ -75,6 +75,9 @@ namespace BlackMisc //! Set the time values void setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs); + //! \private UNIT tests/ASSERT only + const PosArray &getPa() const { return m_pa; } + private: PosArray m_pa; //!< current positions array, latest values last PhysicalQuantities::CLengthUnit m_altitudeUnit;