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

@@ -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

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);
             }
 

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

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<std::array<double, 3>, 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<double>(m_s[0].getAdjustedMSecsSinceEpoch()), static_cast<double>(m_s[1].getAdjustedMSecsSinceEpoch()), static_cast<double>(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<double>(m_currentTimeMsSinceEpoch - m_prevSampleAdjustedTime);
-            const double dt2 = static_cast<double>(m_nextSampleAdjustedTime - m_prevSampleAdjustedTime);
+            const double dt2 = static_cast<double>(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 &currentSituation, 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<double, 3> 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)
             {

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;