From dec0b12ea2b0467f885b2ecadb69cca37f4204c3 Mon Sep 17 00:00:00 2001 From: Klaus Basan Date: Wed, 10 Oct 2018 15:03:31 +0200 Subject: [PATCH] Ref T397, checks on vector to find root cause NaN/inf issue * sometimes very large numbers (xxE38) in vector * sometimes vectors which look correct, but exceed [-1,1] --- src/blackcore/airspacemonitor.cpp | 14 ++++++- src/blackgui/views/simulatedaircraftview.cpp | 2 +- src/blackmisc/aviation/aircraftsituation.cpp | 14 +++++-- src/blackmisc/aviation/aircraftsituation.h | 5 ++- src/blackmisc/geo/coordinategeodetic.cpp | 42 ++++++++++++++++++- src/blackmisc/geo/coordinategeodetic.h | 9 ++++ src/blackmisc/simulation/interpolator.cpp | 10 +++++ .../simulation/interpolatorspline.cpp | 7 ++++ src/blackmisc/simulation/interpolatorspline.h | 2 +- .../simulation/remoteaircraftprovider.cpp | 2 +- 10 files changed, 98 insertions(+), 9 deletions(-) diff --git a/src/blackcore/airspacemonitor.cpp b/src/blackcore/airspacemonitor.cpp index 9c9fa2b29..37c9500ab 100644 --- a/src/blackcore/airspacemonitor.cpp +++ b/src/blackcore/airspacemonitor.cpp @@ -38,6 +38,7 @@ #include "blackmisc/variant.h" #include "blackmisc/verify.h" #include "blackmisc/worker.h" +#include "blackconfig/buildconfig.h" #include #include @@ -49,6 +50,7 @@ #include #include +using namespace BlackConfig; using namespace BlackMisc; using namespace BlackMisc::Audio; using namespace BlackMisc::Aviation; @@ -930,15 +932,23 @@ namespace BlackCore if (!this->isConnectedAndNotShuttingDown()) { return; } const CCallsign callsign(situation.getCallsign()); + + // checks Q_ASSERT_X(!callsign.isEmpty(), Q_FUNC_INFO, "Empty callsign"); + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + Q_ASSERT_X(!situation.isNaNVectorDouble(), Q_FUNC_INFO, "Detected NaN"); + Q_ASSERT_X(!situation.isInfVectorDouble(), Q_FUNC_INFO, "Detected inf"); + Q_ASSERT_X(situation.isValidVectorRange(), Q_FUNC_INFO, "out of range [-1,1]"); + } + // Interim packets do not have groundspeed, hence set the last known value. // If there is no full position available yet, throw this interim position away. CAircraftSituation interimSituation(situation); CAircraftSituationList history = this->remoteAircraftSituations(callsign); if (history.empty()) { return; } // we need one full situation at least const CAircraftSituation lastSituation = history.latestObject(); - const bool samePosition = (lastSituation.getPosition() == interimSituation.getPosition()); // changed position, continue and copy values interimSituation.setCurrentUtcTime(); @@ -949,6 +959,8 @@ namespace BlackCore // if we have no aircraft in range yet, we stop here if (!this->isAircraftInRange(callsign)) { return; } + + const bool samePosition = lastSituation.equalNormalVectorDouble(interimSituation); if (samePosition) { return; } // nothing to update // update aircraft diff --git a/src/blackgui/views/simulatedaircraftview.cpp b/src/blackgui/views/simulatedaircraftview.cpp index 917bb80c6..30391ef86 100644 --- a/src/blackgui/views/simulatedaircraftview.cpp +++ b/src/blackgui/views/simulatedaircraftview.cpp @@ -90,7 +90,7 @@ namespace BlackGui } if (m_withMenuFastPosition) { - menuActions.addAction(CIcons::globe16(), aircraft.fastPositionUpdates() ? "Normal updates" : "Fast position updates", CMenuAction::pathClientSimulationTransfer(), { this, &CSimulatedAircraftView::toggleFastPositionUpdates }); + menuActions.addAction(CIcons::globe16(), aircraft.fastPositionUpdates() ? "Normal updates" : "Fast position updates (send)", CMenuAction::pathClientSimulationTransfer(), { this, &CSimulatedAircraftView::toggleFastPositionUpdates }); } const bool any = m_withMenuEnableAircraft || m_withMenuFastPosition || m_withMenuHighlightAndFollow || m_withMenuEnableGndFlag; if (any && (sApp && sApp->isDeveloperFlagSet())) diff --git a/src/blackmisc/aviation/aircraftsituation.cpp b/src/blackmisc/aviation/aircraftsituation.cpp index c67c1e504..77ce59c10 100644 --- a/src/blackmisc/aviation/aircraftsituation.cpp +++ b/src/blackmisc/aviation/aircraftsituation.cpp @@ -243,16 +243,24 @@ namespace BlackMisc CElevationPlane CAircraftSituation::interpolatedElevation(const CAircraftSituation &situation, const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, const CLength &distance) { - if (oldSituation.isNull() || newSituation.isNull()) { return CAircraftSituation::null(); } + if (oldSituation.isNull() || newSituation.isNull()) { return CElevationPlane::null(); } + if (!oldSituation.hasGroundElevation() || !newSituation.hasGroundElevation()) { return CElevationPlane::null(); } if (oldSituation.equalNormalVectorDouble(newSituation)) { return newSituation.getGroundElevationPlane(); } const double newElvFt = newSituation.getGroundElevation().value(CLengthUnit::ft()); const double oldElvFt = oldSituation.getGroundElevation().value(CLengthUnit::ft()); const double deltaElvFt = newElvFt - oldElvFt; - if (deltaElvFt > 25) { return CElevationPlane::null(); } + if (deltaElvFt > MaxDeltaElevationFt) { return CElevationPlane::null(); } // threshold, interpolation not possible if (!situation.isNull()) { + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + Q_ASSERT_X(situation.isValidVectorRange(), Q_FUNC_INFO, "Invalid range"); + Q_ASSERT_X(oldSituation.isValidVectorRange(), Q_FUNC_INFO, "Invalid range"); + Q_ASSERT_X(newSituation.isValidVectorRange(), Q_FUNC_INFO, "Invalid range"); + } + const double distanceSituationNewM = situation.calculateGreatCircleDistance(newSituation).value(CLengthUnit::m()); if (distanceSituationNewM < 5.0) { return newSituation.getGroundElevationPlane(); } @@ -261,7 +269,7 @@ namespace BlackMisc const double distRatio = distanceSituationNewM / distanceOldNewM; - // very close to the situations we return tehir elevation + // very close to the situations we return their elevation if (distRatio < 0.05) { return newSituation.getGroundElevationPlane(); } if (distRatio > 0.95) { return oldSituation.getGroundElevationPlane(); } diff --git a/src/blackmisc/aviation/aircraftsituation.h b/src/blackmisc/aviation/aircraftsituation.h index 3f6c0e258..541dbfe19 100644 --- a/src/blackmisc/aviation/aircraftsituation.h +++ b/src/blackmisc/aviation/aircraftsituation.h @@ -550,9 +550,12 @@ namespace BlackMisc static bool extrapolateElevation(CAircraftSituation &newSituation, const CAircraftSituation &oldSituation, const CAircraftSituation &olderSituation, const CAircraftSituationChange &oldChange); //! Interpolate between the 2 situations for situation - //! \remark NULL if there are no two elevations + //! \remark NULL if there are no two elevations or threshold MaxDeltaElevationFt is exceeded static Geo::CElevationPlane interpolatedElevation(const CAircraftSituation &situation, const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, const PhysicalQuantities::CLength &distance = PhysicalQuantities::CLength::null()); + //! Threshold until we interpolate elevations + static constexpr double MaxDeltaElevationFt = 25.0; + //! Register metadata static void registerMetadata(); diff --git a/src/blackmisc/geo/coordinategeodetic.cpp b/src/blackmisc/geo/coordinategeodetic.cpp index 1e8019c38..e04321120 100644 --- a/src/blackmisc/geo/coordinategeodetic.cpp +++ b/src/blackmisc/geo/coordinategeodetic.cpp @@ -46,10 +46,12 @@ namespace BlackMisc if (coordinate1.isNull() || coordinate2.isNull()) { return CLength::null(); } // if (coordinate1.equalNormalVectorDouble(coordinate2)) { return CLength(0, CLengthUnit::defaultUnit()); } static const float earthRadiusMeters = 6371000.8f; + const QVector3D v1 = coordinate1.normalVector(); const QVector3D v2 = coordinate2.normalVector(); + Q_ASSERT_X(std::isfinite(v1.x()) && std::isfinite(v1.y()) && std::isfinite(v1.z()), Q_FUNC_INFO, "Distance calculation: v1 non-finite argument"); + Q_ASSERT_X(std::isfinite(v2.x()) && std::isfinite(v2.y()) && std::isfinite(v2.z()), Q_FUNC_INFO, "Distance calculation: v2 non-finite argument"); - Q_ASSERT_X(!std::isnan(v1.x()) && !std::isnan(v1.y()) && !std::isnan(v1.z()) && !std::isnan(v2.x()) && !std::isnan(v2.y()) && !std::isnan(v2.z()), Q_FUNC_INFO, "Distance calculation: NaN in argument"); const float d = earthRadiusMeters * std::atan2(QVector3D::crossProduct(v1, v2).length(), QVector3D::dotProduct(v1, v2)); BLACK_VERIFY_X(!std::isnan(d), Q_FUNC_INFO, "Distance calculation: NaN in result"); @@ -183,6 +185,44 @@ namespace BlackMisc this->geodeticHeight().valueRoundedWithUnit(CLengthUnit::ft(), 2, i18n)); } + bool ICoordinateGeodetic::isNaNVector() const + { + const QVector3D v = this->normalVector(); + return std::isnan(v.x()) || std::isnan(v.y()) || std::isnan(v.z()); + } + + bool ICoordinateGeodetic::isNaNVectorDouble() const + { + const std::array v = this->normalVectorDouble(); + return std::isnan(v[0]) || std::isnan(v[1]) || std::isnan(v[2]); + } + + bool ICoordinateGeodetic::isInfVector() const + { + const QVector3D v = this->normalVector(); + return std::isinf(v.x()) || std::isinf(v.y()) || std::isinf(v.z()); + } + + bool ICoordinateGeodetic::isInfVectorDouble() const + { + const std::array v = this->normalVectorDouble(); + return std::isinf(v[0]) || std::isinf(v[1]) || std::isinf(v[2]); + } + + bool ICoordinateGeodetic::isValidVectorRange() const + { + // inf is out of range, comparing nans is always false + const std::array v = this->normalVectorDouble(); + return isValidVector(v); + } + + bool ICoordinateGeodetic::isValidVector(const std::array &v) + { + constexpr double l = 1.00001; // because of interpolation + return v[0] <= l && v[1] <= l && v[2] <= l && + v[0] >= -l && v[1] >= -l && v[2] >= -l; + } + CVariant CCoordinateGeodetic::propertyByIndex(const BlackMisc::CPropertyIndex &index) const { if (index.isMyself()) { return CVariant::from(*this); } diff --git a/src/blackmisc/geo/coordinategeodetic.h b/src/blackmisc/geo/coordinategeodetic.h index 1f3e55221..81589051d 100644 --- a/src/blackmisc/geo/coordinategeodetic.h +++ b/src/blackmisc/geo/coordinategeodetic.h @@ -127,6 +127,15 @@ namespace BlackMisc //! \copydoc Mixin::String::toQString QString convertToQString(bool i18n = false) const; + //! Check values @{ + bool isNaNVector() const; + bool isNaNVectorDouble() const; + bool isInfVector() const; + bool isInfVectorDouble() const; + bool isValidVectorRange() const; + static bool isValidVector(const std::array &v); + //! @} + protected: //! Can given index be handled? static bool canHandleIndex(const CPropertyIndex &index); diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index 153e24dc9..e72fe626c 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -196,6 +196,10 @@ namespace BlackMisc // 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 @@ -446,6 +450,7 @@ namespace BlackMisc CAircraftSituation CInterpolator::initInterpolatedSituation(const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation) const { if (m_currentSituations.isEmpty()) { return CAircraftSituation::null(); } + CAircraftSituation currentSituation = m_lastSituation.isNull() ? m_currentSituations.front() : m_lastSituation; if (currentSituation.getCallsign() != m_callsign) { @@ -453,6 +458,11 @@ namespace BlackMisc currentSituation.setCallsign(m_callsign); } + if (CBuildConfig::isLocalDeveloperDebugBuild()) + { + Q_ASSERT_X(currentSituation.isValidVectorRange(), Q_FUNC_INFO, "Invalid range"); + } + // preset elevation here, as we do not know where the situation will be after the interpolation step! const bool preset = currentSituation.presetGroundElevation(oldSituation, newSituation, m_pastSituationsChange); Q_UNUSED(preset); diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index 0b987e84c..a142e3c0d 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -281,6 +281,13 @@ namespace BlackMisc 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 + } + CAircraftSituation newSituation(currentSituation); const std::array normalVector = {{ newX, newY, newZ }}; const CCoordinateGeodetic currentPosition(normalVector); diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h index 8e9431b56..0e0e7935c 100644 --- a/src/blackmisc/simulation/interpolatorspline.h +++ b/src/blackmisc/simulation/interpolatorspline.h @@ -76,7 +76,7 @@ namespace BlackMisc void setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs); private: - PosArray m_pa; //! current positions array, latest values last + PosArray m_pa; //!< current positions array, latest values last PhysicalQuantities::CLengthUnit m_altitudeUnit; qint64 m_currentTimeMsSinceEpoc { -1 }; }; diff --git a/src/blackmisc/simulation/remoteaircraftprovider.cpp b/src/blackmisc/simulation/remoteaircraftprovider.cpp index fe08d04d3..51106b244 100644 --- a/src/blackmisc/simulation/remoteaircraftprovider.cpp +++ b/src/blackmisc/simulation/remoteaircraftprovider.cpp @@ -262,7 +262,7 @@ namespace BlackMisc BLACK_VERIFY_X(situation.getTimeOffsetMs() > 0, Q_FUNC_INFO, "Missing offset"); } - // add offset (for testing only) + // add altitude offset (for testing only) CAircraftSituation situationCorrected(allowTestOffset ? this->addTestAltitudeOffsetToSituation(situation) : situation); // CG, model