Ref T260, interpolator improvements

* use last interpolated situation when recalculating interpolant
* flag if interpolation factor is to be calculated, or guessed

src/blackmisc/simulation/interpolator.cpp

@@ -148,17 +148,26 @@ namespace BlackMisc
 
             // Pitch bank heading
             // first, so follow up steps could use those values
-            const auto pbh = interpolant.pbh();
+            const CInterpolatorPbh pbh = interpolant.pbh();
             currentSituation.setHeading(pbh.getHeading());
             currentSituation.setPitch(pbh.getPitch());
             currentSituation.setBank(pbh.getBank());
             currentSituation.setGroundSpeed(pbh.getGroundSpeed());
 
             // use derived interpolant function
-            currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation);
+            const bool interpolateGndFlag = pbh.getNewSituation().hasGroundDetailsForGndInterpolation() && pbh.getOldSituation().hasGroundDetailsForGndInterpolation();
+            currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation, interpolateGndFlag);
+            if (!interpolateGndFlag) { currentSituation.guessOnGround(m_model.isVtol(), m_cg); }
 
             // correct itself
-            const CAircraftSituation::AltitudeCorrection altCorrection = currentSituation.correctAltitude(m_cg, true);
+            CAircraftSituation::AltitudeCorrection altCorrection = CAircraftSituation::NoCorrection;
+            if (!interpolateGndFlag && currentSituation.getOnGroundDetails() != CAircraftSituation::OnGroundByGuessing)
+            {
+                // just in case
+                altCorrection = currentSituation.correctAltitude(m_cg, true);
+            }
+
+            // status
             status.setInterpolatedAndCheckSituation(true, currentSituation);
 
             // logging

src/blackmisc/simulation/interpolatorlinear.cpp

@@ -49,7 +49,7 @@ namespace BlackMisc
             m_pbh(m_simulationTimeFraction, situation1, situation2)
         {}
 
-        CAircraftSituation CInterpolatorLinear::Interpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation) const
+        CAircraftSituation CInterpolatorLinear::Interpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const
         {
             const std::array<double, 3> oldVec(m_oldSituation.getPosition().normalVectorDouble());
             const std::array<double, 3> newVec(m_newSituation.getPosition().normalVectorDouble());
@@ -75,17 +75,20 @@ namespace BlackMisc
             newSituation.setAltitude(altitude);
             newSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
 
-            const double oldGroundFactor = m_oldSituation.getOnGroundFactor();
+            if (interpolateGndFactor)
-            const double newGroundFactor = m_newSituation.getOnGroundFactor();
-            do
             {
-                if (gfEqualAirborne(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(false); break; }
+                const double oldGroundFactor = m_oldSituation.getOnGroundFactor();
-                if (gfEqualOnGround(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(true); break; }
+                const double newGroundFactor = m_newSituation.getOnGroundFactor();
-                const double groundFactor = (newGroundFactor - oldGroundFactor) * m_simulationTimeFraction + oldGroundFactor;
+                do
-                newSituation.setOnGroundFactor(groundFactor);
+                {
-                newSituation.setOnGroundFromGroundFactorFromInterpolation();
+                    if (gfEqualAirborne(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(false); break; }
+                    if (gfEqualOnGround(oldGroundFactor, newGroundFactor)) { newSituation.setOnGround(true); break; }
+                    const double groundFactor = (newGroundFactor - oldGroundFactor) * m_simulationTimeFraction + oldGroundFactor;
+                    newSituation.setOnGroundFactor(groundFactor);
+                    newSituation.setOnGroundFromGroundFactorFromInterpolation();
+                }
+                while (false);
             }
-            while (false);
             return newSituation;
         }
 

src/blackmisc/simulation/interpolatorlinear.h

@@ -47,7 +47,7 @@ namespace BlackMisc
                 //! @}
 
                 //! Perform the interpolation
-                Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation) const;
+                Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor) const;
 
                 //! Interpolator for pitch, bank, heading, groundspeed
                 const CInterpolatorPbh &pbh() const { return m_pbh; }

src/blackmisc/simulation/interpolatorspline.cpp

@@ -105,7 +105,8 @@ namespace BlackMisc
             Q_UNUSED(setup);
 
             // recalculate derivatives only if they changed
-            bool recalculate = currentTimeMsSinceEpoc > m_nextSampleAdjustedTime; // new step
+            const bool newStep = currentTimeMsSinceEpoc > m_nextSampleAdjustedTime; // new step
+            bool recalculate = newStep;
             const qint64 lastModified = this->situationsLastModified(m_callsign);
             if (!recalculate && (lastModified > m_situationsLastModifiedUsed) && this->isAnySituationNearGroundRelevant())
             {
@@ -136,6 +137,12 @@ namespace BlackMisc
                 if (situationsNewer.isEmpty() || situationsOlder.size() < 2) { return m_interpolant; }
                 m_s = std::array<CAircraftSituation, 3> {{ *(situationsOlder.begin() + 1), *situationsOlder.begin(), *(situationsNewer.end() - 1) }};
 
+                // we interpolate from 1 -> 2, 0 for smoother interpolation
+                if (newStep && !m_lastInterpolation.isNull())
+                {
+                    m_s[1] = m_lastInterpolation; // true only for the moment we create a new step
+                }
+
                 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
@@ -261,7 +268,7 @@ namespace BlackMisc
             return true;
         }
 
-        CAircraftSituation CInterpolatorSpline::Interpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation) const
+        CAircraftSituation CInterpolatorSpline::Interpolant::interpolatePositionAndAltitude(const CAircraftSituation &currentSituation, bool interpolateGndFactor) const
         {
             const double t1 = m_pa.t[1];
             const double t2 = m_pa.t[2];
@@ -269,7 +276,7 @@ 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]);
 
-            CAircraftSituation newSituation(situation);
+            CAircraftSituation newSituation(currentSituation);
             const std::array<double, 3> normalVector = {{ newX, newY, newZ }};
             const CCoordinateGeodetic currentPosition(normalVector);
 
@@ -280,17 +287,21 @@ namespace BlackMisc
             newSituation.setAltitude(alt);
             newSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
 
-            const double gnd1 = m_pa.gnd[1];
+            if (interpolateGndFactor)
-            const double gnd2 = m_pa.gnd[2];
-            do
             {
-                if (gfEqualAirborne(gnd1, gnd2)) { newSituation.setOnGround(false); break; }
+                const double gnd1 = m_pa.gnd[1];
-                if (gfEqualOnGround(gnd1, gnd2)) { newSituation.setOnGround(true); break; }
+                const double gnd2 = m_pa.gnd[2];
-                const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]);
+                do
-                newSituation.setOnGroundFactor(newGnd);
+                {
-                newSituation.setOnGroundFromGroundFactorFromInterpolation();
+                    newSituation.setOnGroundDetails(CAircraftSituation::OnGroundByInterpolation);
+                    if (gfEqualAirborne(gnd1, gnd2)) { newSituation.setOnGround(false); break; }
+                    if (gfEqualOnGround(gnd1, gnd2)) { newSituation.setOnGround(true); break; }
+                    const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]);
+                    newSituation.setOnGroundFactor(newGnd);
+                    newSituation.setOnGroundFromGroundFactorFromInterpolation();
+                }
+                while (false);
             }
-            while (false);
             return newSituation;
         }
 

src/blackmisc/simulation/interpolatorspline.h

@@ -60,7 +60,7 @@ namespace BlackMisc
                     m_pa(pa), m_altitudeUnit(altitudeUnit), m_pbh(pbh) {}
 
                 //! Perform the interpolation
-                Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation) const;
+                Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &currentSituation, bool interpolateGndFactor) const;
 
                 //! Interpolator for pitch, bank, heading, groundspeed
                 const CInterpolatorPbh &pbh() const { return m_pbh; }