From 5243682124d63b675a27df19988255251cf0039f Mon Sep 17 00:00:00 2001 From: Klaus Basan Date: Wed, 18 Apr 2018 05:08:45 +0200 Subject: [PATCH] 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 | 15 ++++++-- .../simulation/interpolatorlinear.cpp | 23 ++++++------ src/blackmisc/simulation/interpolatorlinear.h | 2 +- .../simulation/interpolatorspline.cpp | 35 ++++++++++++------- src/blackmisc/simulation/interpolatorspline.h | 2 +- 5 files changed, 50 insertions(+), 27 deletions(-) diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index a03ebb0f5..9be0b1d08 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/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 diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index a1e0372fa..cc2bd8c89 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/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 oldVec(m_oldSituation.getPosition().normalVectorDouble()); const std::array newVec(m_newSituation.getPosition().normalVectorDouble()); @@ -75,17 +75,20 @@ namespace BlackMisc newSituation.setAltitude(altitude); newSituation.setMSecsSinceEpoch(this->getInterpolatedTime()); - const double oldGroundFactor = m_oldSituation.getOnGroundFactor(); - const double newGroundFactor = m_newSituation.getOnGroundFactor(); - do + if (interpolateGndFactor) { - 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(); + const double oldGroundFactor = m_oldSituation.getOnGroundFactor(); + const double newGroundFactor = m_newSituation.getOnGroundFactor(); + do + { + 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; } diff --git a/src/blackmisc/simulation/interpolatorlinear.h b/src/blackmisc/simulation/interpolatorlinear.h index 9332b9a6d..e52159256 100644 --- a/src/blackmisc/simulation/interpolatorlinear.h +++ b/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; } diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index 878e2220d..f3bdeb691 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/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 {{ *(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, 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 ¤tSituation, 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 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]; - const double gnd2 = m_pa.gnd[2]; - do + if (interpolateGndFactor) { - 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(); + const double gnd1 = m_pa.gnd[1]; + const double gnd2 = m_pa.gnd[2]; + do + { + 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; } diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h index c71766889..a76f306fd 100644 --- a/src/blackmisc/simulation/interpolatorspline.h +++ b/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 ¤tSituation, bool interpolateGndFactor) const; //! Interpolator for pitch, bank, heading, groundspeed const CInterpolatorPbh &pbh() const { return m_pbh; }