diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index 71a431f50..17735004c 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -258,7 +258,13 @@ namespace BlackMisc::Simulation // use derived interpolant function const bool interpolateGndFlag = pbh.getEndSituation().hasGroundDetailsForGndInterpolation() && pbh.getStartSituation().hasGroundDetailsForGndInterpolation(); - currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation, interpolateGndFlag); + currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation); + + if (interpolateGndFlag) + { + currentSituation.setOnGroundInfo(interpolant.interpolateGroundFactor()); + } + if (currentSituation.isNull()) { break; } // if we get here and the vector is invalid it means we haven't handled it correctly in one of the interpolators diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index 4e7a32468..9730634d3 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/src/blackmisc/simulation/interpolatorlinear.cpp @@ -45,7 +45,7 @@ namespace BlackMisc::Simulation void CInterpolatorLinear::anchor() {} - CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const + CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation) const { const std::array startVec(m_startSituation.getPosition().normalVectorDouble()); const std::array endVec(m_endSituation.getPosition().normalVectorDouble()); @@ -82,27 +82,29 @@ namespace BlackMisc::Simulation interpolatedSituation.setAltitude(altitude); interpolatedSituation.setMSecsSinceEpoch(this->getInterpolatedTime()); - if (interpolateGndFactor) - { - const double startGroundFactor = m_startSituation.getOnGroundInfo().getGroundFactor(); - const double endGroundFactor = m_endSituation.getOnGroundInfo().getGroundFactor(); - if (CAircraftSituation::isGfEqualAirborne(startGroundFactor, endGroundFactor)) - { - interpolatedSituation.setOnGroundInfo({ COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation }); - } - else if (CAircraftSituation::isGfEqualOnGround(startGroundFactor, endGroundFactor)) - { - interpolatedSituation.setOnGroundInfo({ COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation }); - } - else - { - const double interpolatedGroundFactor = (endGroundFactor - startGroundFactor) * tf + startGroundFactor; - interpolatedSituation.setOnGroundInfo(COnGroundInfo(interpolatedGroundFactor)); - } - } return interpolatedSituation; } + Aviation::COnGroundInfo CInterpolatorLinear::CInterpolant::interpolateGroundFactor() const + { + const double startGroundFactor = m_startSituation.getOnGroundInfo().getGroundFactor(); + const double endGroundFactor = m_endSituation.getOnGroundInfo().getGroundFactor(); + if (CAircraftSituation::isGfEqualAirborne(startGroundFactor, endGroundFactor)) + { + return { COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation }; + } + else if (CAircraftSituation::isGfEqualOnGround(startGroundFactor, endGroundFactor)) + { + return { COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation }; + } + else + { + const double tf = clampValidTimeFraction(m_simulationTimeFraction); + const double interpolatedGroundFactor = (endGroundFactor - startGroundFactor) * tf + startGroundFactor; + return COnGroundInfo(interpolatedGroundFactor); + } + } + CInterpolatorLinear::CInterpolant CInterpolatorLinear::getInterpolant(SituationLog &log) { // set default situations diff --git a/src/blackmisc/simulation/interpolatorlinear.h b/src/blackmisc/simulation/interpolatorlinear.h index 73d38773e..c39cf3cb2 100644 --- a/src/blackmisc/simulation/interpolatorlinear.h +++ b/src/blackmisc/simulation/interpolatorlinear.h @@ -49,9 +49,11 @@ namespace BlackMisc //! Perform the interpolation //! \param situation situation used as a base for interpolation. Contains for example the already interpolated PBH. - //! \param interpolateGndFactor whether to interpolate the GND factor. - //! \return \p situation with interpolated position and altitude, updated timestamp and possibly interpolated GND factor - Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor) const; + //! \return \p situation with interpolated position and altitude and updated timestamp + Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation) const; + + //! Interpolate the ground information/factor + Aviation::COnGroundInfo interpolateGroundFactor() const; //! Start situation const Aviation::CAircraftSituation &getStartSituation() const { return m_startSituation; } diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index 0fbbcdebe..c7f2fca89 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -315,7 +315,7 @@ namespace BlackMisc::Simulation m_situationsAvailable = pa.size(); } - CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation, bool interpolateGndFactor) const + CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation) const { const double t1 = m_pa.t[1]; const double t2 = m_pa.t[2]; // latest (adjusted) @@ -361,28 +361,34 @@ namespace BlackMisc::Simulation newSituation.setAltitude(alt); newSituation.setMSecsSinceEpoch(this->getInterpolatedTime()); - if (interpolateGndFactor) - { - const double gnd1 = m_pa.gnd[1]; - const double gnd2 = m_pa.gnd[2]; // latest - - if (CAircraftSituation::isGfEqualAirborne(gnd1, gnd2)) - { - newSituation.setOnGroundInfo({ COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation }); - } - else if (CAircraftSituation::isGfEqualOnGround(gnd1, gnd2)) - { - newSituation.setOnGroundInfo({ COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation }); - } - else - { - const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]); - newSituation.setOnGroundInfo(COnGroundInfo(newGnd)); - } - } return newSituation; } + Aviation::COnGroundInfo CInterpolatorSpline::CInterpolant::interpolateGroundFactor() const + { + const double t1 = m_pa.t[1]; + const double t2 = m_pa.t[2]; // latest (adjusted) + bool valid = (t1 < t2) && (m_currentTimeMsSinceEpoc >= t1) && (m_currentTimeMsSinceEpoc < t2); + if (!valid) { return { COnGroundInfo::OnGroundSituationUnknown, COnGroundInfo::NotSetGroundDetails }; } + + const double gnd1 = m_pa.gnd[1]; + const double gnd2 = m_pa.gnd[2]; // latest + + if (CAircraftSituation::isGfEqualAirborne(gnd1, gnd2)) + { + return { COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation }; + } + else if (CAircraftSituation::isGfEqualOnGround(gnd1, gnd2)) + { + return { COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation }; + } + else + { + const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]); + return COnGroundInfo(newGnd); + } + } + void CInterpolatorSpline::CInterpolant::setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs) { m_currentTimeMsSinceEpoc = currentTimeMs; diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h index 710e492a0..6721a1a63 100644 --- a/src/blackmisc/simulation/interpolatorspline.h +++ b/src/blackmisc/simulation/interpolatorspline.h @@ -65,9 +65,11 @@ namespace BlackMisc::Simulation //! Perform the interpolation //! \param situation situation used as a base for interpolation. Contains for example the already interpolated PBH. - //! \param interpolateGndFactor whether to interpolate the GND factor. - //! \return \p situation with interpolated position and altitude, updated timestamp and possibly interpolated GND factor - Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation, bool interpolateGndFactor) const; + //! \return \p situation with interpolated position and altitude and updated timestamp + Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation) const; + + //! Interpolate the ground information/factor + Aviation::COnGroundInfo interpolateGroundFactor() const; //! Set the time values void setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs);