diff --git a/src/blackmisc/simulation/interpolant.h b/src/blackmisc/simulation/interpolant.h index 80a03833a..13a76cd61 100644 --- a/src/blackmisc/simulation/interpolant.h +++ b/src/blackmisc/simulation/interpolant.h @@ -18,6 +18,7 @@ namespace BlackMisc { namespace Simulation { + //! Interpolant interface class IInterpolant { public: @@ -41,7 +42,7 @@ namespace BlackMisc IInterpolant(qint64 interpolatedTime, int situationsAvailable) : m_interpolatedTime(interpolatedTime), m_situationsAvailable(situationsAvailable) {} qint64 m_interpolatedTime = -1; //!< "Real time "of interpolated situation - int m_situationsAvailable = 0; //!< used situations + int m_situationsAvailable = 0; //!< used situations CInterpolatorPbh m_pbh; //!< the used PBH interpolator }; } // namespace diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index 019a1528d..111af59a5 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -71,6 +71,21 @@ namespace BlackMisc return f; } + template + CAircraftSituationList CInterpolator::remoteAircraftSituationsAndChange(bool useSceneryOffset) + { + CAircraftSituationList validSituations = this->remoteAircraftSituations(m_callsign); + m_situationChange = CAircraftSituationChange(validSituations, true, true); + if (useSceneryOffset && m_situationChange.hasSceneryDeviation() && m_model.hasCG()) + { + const CLength os = m_situationChange.getGuessedSceneryDeviation(m_model.getCG()); + validSituations.addAltitudeOffset(os); + m_situationChange = CAircraftSituationChange(validSituations, true, true); // recalculate + m_lastSceneryOffset = os; + } + return validSituations; + } + template void CInterpolator::deferredInit() { diff --git a/src/blackmisc/simulation/interpolator.h b/src/blackmisc/simulation/interpolator.h index 458fb1eb7..8c7dff4ac 100644 --- a/src/blackmisc/simulation/interpolator.h +++ b/src/blackmisc/simulation/interpolator.h @@ -41,9 +41,9 @@ namespace BlackMisc //! Interpolator, calculation inbetween positions template class CInterpolator : - public CSimulationEnvironmentAware, - public CInterpolationSetupAware, - public CRemoteAircraftAware + protected CSimulationEnvironmentAware, + protected CInterpolationSetupAware, + protected CRemoteAircraftAware { public: //! Log categories @@ -109,13 +109,17 @@ namespace BlackMisc //! \sa BlackMisc::Aviation::CAircraftSituation::setOnGroundFromGroundFactorFromInterpolation static double groundInterpolationFactor(); - const Aviation::CCallsign m_callsign; //!< corresponding callsign CAircraftModel m_model; //!< corresponding model + const Aviation::CCallsign m_callsign; //!< corresponding callsign Aviation::CAircraftSituation m_lastInterpolation { Aviation::CAircraftSituation::null() }; //!< latest interpolation Aviation::CAircraftSituationChange m_situationChange; //!< situations change + PhysicalQuantities::CLength m_lastSceneryOffset = PhysicalQuantities::CLength::null(); qint64 m_situationsLastModifiedUsed { -1 }; //!< based on situations last updated int m_interpolatedSituationsCounter = 0; //!< counter for each interpolated situations: statistics, every n-th interpolation .... + //! Get situations and calculate change, also correct altitudes if applicable + Aviation::CAircraftSituationList remoteAircraftSituationsAndChange(bool useSceneryOffset); + //! Verify gnd flag, times, ... true means "OK" bool verifyInterpolationSituations(const Aviation::CAircraftSituation &oldest, const Aviation::CAircraftSituation &newer, const Aviation::CAircraftSituation &latest, const CInterpolationAndRenderingSetupPerCallsign &setup = CInterpolationAndRenderingSetupPerCallsign::null()); diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index bd874b774..b76190fb2 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/src/blackmisc/simulation/interpolatorlinear.cpp @@ -36,20 +36,25 @@ namespace BlackMisc { namespace Simulation { - CInterpolatorLinear::Interpolant::Interpolant(const CAircraftSituation &situation) : - m_situationsAvailable(1), m_oldSituation(situation), - m_pbh(0, situation, situation) - {} + CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &situation) : + IInterpolant(1, CInterpolatorPbh(0, situation, situation)), + m_oldSituation(situation) + { } - CInterpolatorLinear::Interpolant::Interpolant(const CAircraftSituation &situation1, const CAircraftSituation &situation2, double timeFraction, qint64 interpolatedTime) : - m_situationsAvailable(2), + CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &situation, const CInterpolatorPbh &pbh) : + IInterpolant(1, pbh), + m_oldSituation(situation) + { } + + CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &situation1, const CAircraftSituation &situation2, double timeFraction, qint64 interpolatedTime) : + IInterpolant(interpolatedTime, 2), m_oldSituation(situation1), m_newSituation(situation2), - m_simulationTimeFraction(timeFraction), - m_interpolatedTime(interpolatedTime), - m_pbh(m_simulationTimeFraction, situation1, situation2) - {} + m_simulationTimeFraction(timeFraction) + { + m_pbh = CInterpolatorPbh(m_simulationTimeFraction, situation1, situation2); + } - CAircraftSituation CInterpolatorLinear::Interpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const + CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const { const std::array oldVec(m_oldSituation.getPosition().normalVectorDouble()); const std::array newVec(m_newSituation.getPosition().normalVectorDouble()); @@ -92,7 +97,7 @@ namespace BlackMisc return newSituation; } - CInterpolatorLinear::Interpolant CInterpolatorLinear::getInterpolant( + CInterpolatorLinear::CInterpolant CInterpolatorLinear::getInterpolant( qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetupPerCallsign &setup, CInterpolationStatus &status, SituationLog &log) @@ -100,82 +105,84 @@ namespace BlackMisc Q_UNUSED(setup); status.reset(); - // with the latest updates of T243 the order and the offsets are supposed to be correct - // so even mixing fast/slow updates shall work - const CAircraftSituationList validSituations = this->remoteAircraftSituations(m_callsign); // if needed, we could also copy here - if (!CBuildConfig::isReleaseBuild()) - { - BLACK_VERIFY_X(validSituations.isSortedAdjustedLatestFirstWithoutNullPositions(), Q_FUNC_INFO, "Wrong sort order"); - Q_ASSERT_X(validSituations.size() <= IRemoteAircraftProvider::MaxSituationsPerCallsign, Q_FUNC_INFO, "Wrong size"); - } - const qint64 tsLastModified = this->situationsLastModified(m_callsign); + + // set default situations + CAircraftSituation oldSituation = m_interpolant.getOldSituation(); + CAircraftSituation newSituation = m_interpolant.getNewSituation(); + if (m_situationsLastModifiedUsed < tsLastModified || m_situationChange.isNull()) { m_situationsLastModifiedUsed = tsLastModified; - m_situationChange = CAircraftSituationChange(validSituations, true, true); - } - // find the first situation earlier than the current time - const auto pivot = std::partition_point(validSituations.begin(), validSituations.end(), [ = ](auto &&s) { return s.getAdjustedMSecsSinceEpoch() > currentTimeMsSinceEpoc; }); - const auto situationsNewer = makeRange(validSituations.begin(), pivot); - const auto situationsOlder = makeRange(pivot, validSituations.end()); - - // interpolation situations - CAircraftSituation oldSituation; - CAircraftSituation newSituation; - - // latest first, now 00:20 split time - // time pos - // 00:25 10 newer - // 00:20 11 newer - // <----- split - // 00:15 12 older - // 00:10 13 older - // 00:05 14 older - - // The first condition covers a situation, when there are no before / after situations. - // We just place at the last position until we get before / after situations - if (situationsOlder.isEmpty() || situationsNewer.isEmpty()) - { - // no before situations - if (situationsOlder.isEmpty()) + // with the latest updates of T243 the order and the offsets are supposed to be correct + // so even mixing fast/slow updates shall work + const CAircraftSituationList validSituations = this->remoteAircraftSituationsAndChange(true); + if (!CBuildConfig::isReleaseBuild()) { - const CAircraftSituation currentSituation(*(situationsNewer.end() - 1)); // oldest newest - status.setInterpolatedAndCheckSituation(false, currentSituation); - return currentSituation; + BLACK_VERIFY_X(validSituations.isSortedAdjustedLatestFirstWithoutNullPositions(), Q_FUNC_INFO, "Wrong sort order"); + Q_ASSERT_X(validSituations.size() <= IRemoteAircraftProvider::MaxSituationsPerCallsign, Q_FUNC_INFO, "Wrong size"); } - // only one before situation - if (situationsOlder.size() < 2) + // find the first situation earlier than the current time + const auto pivot = std::partition_point(validSituations.begin(), validSituations.end(), [ = ](auto &&s) { return s.getAdjustedMSecsSinceEpoch() > currentTimeMsSinceEpoc; }); + const auto situationsNewer = makeRange(validSituations.begin(), pivot); + const auto situationsOlder = makeRange(pivot, validSituations.end()); + + // latest first, now 00:20 split time + // time pos + // 00:25 10 newer + // 00:20 11 newer + // <----- split + // 00:15 12 older + // 00:10 13 older + // 00:05 14 older + + // The first condition covers a situation, when there are no before / after situations. + // We just place at the last position until we get before / after situations + if (situationsOlder.isEmpty() || situationsNewer.isEmpty()) { - const CAircraftSituation currentSituation(situationsOlder.front()); // latest oldest - status.setInterpolatedAndCheckSituation(false, currentSituation); - return currentSituation; + // no before situations + if (situationsOlder.isEmpty()) + { + const CAircraftSituation currentSituation(*(situationsNewer.end() - 1)); // oldest newest + status.setInterpolatedAndCheckSituation(false, currentSituation); + m_interpolant = { currentSituation }; + return m_interpolant; + } + + // only one before situation + if (situationsOlder.size() < 2) + { + const CAircraftSituation currentSituation(situationsOlder.front()); // latest oldest + status.setInterpolatedAndCheckSituation(false, currentSituation); + m_interpolant = { currentSituation }; + return m_interpolant; + } + + // extrapolate from two before situations + oldSituation = *(situationsOlder.begin() + 1); // before newest + newSituation = situationsOlder.front(); // newest + } + else + { + oldSituation = situationsOlder.front(); // first oldest (aka newest oldest) + newSituation = *(situationsNewer.end() - 1); // latest newest (aka oldest of newer block) + Q_ASSERT(oldSituation.getAdjustedMSecsSinceEpoch() < newSituation.getAdjustedMSecsSinceEpoch()); } - // extrapolate from two before situations - oldSituation = *(situationsOlder.begin() + 1); // before newest - newSituation = situationsOlder.front(); // newest - } - else - { - oldSituation = situationsOlder.front(); // first oldest (aka newest oldest) - newSituation = *(situationsNewer.end() - 1); // latest newest (aka oldest of newer block) - Q_ASSERT(oldSituation.getAdjustedMSecsSinceEpoch() < newSituation.getAdjustedMSecsSinceEpoch()); - } - - // adjust ground if required - if (!oldSituation.canLikelySkipNearGroundInterpolation() && !oldSituation.hasGroundElevation()) - { - const CElevationPlane planeOld = this->findClosestElevationWithinRange(oldSituation, CElevationPlane::singlePointRadius()); - oldSituation.setGroundElevationChecked(planeOld); - } - if (!newSituation.canLikelySkipNearGroundInterpolation() && !newSituation.hasGroundElevation()) - { - const CElevationPlane planeNew = this->findClosestElevationWithinRange(newSituation, CElevationPlane::singlePointRadius()); - newSituation.setGroundElevationChecked(planeNew); - } + // adjust ground if required + if (!oldSituation.canLikelySkipNearGroundInterpolation() && !oldSituation.hasGroundElevation()) + { + const CElevationPlane planeOld = this->findClosestElevationWithinRange(oldSituation, CElevationPlane::singlePointRadius()); + oldSituation.setGroundElevationChecked(planeOld); + } + if (!newSituation.canLikelySkipNearGroundInterpolation() && !newSituation.hasGroundElevation()) + { + const CElevationPlane planeNew = this->findClosestElevationWithinRange(newSituation, CElevationPlane::singlePointRadius()); + newSituation.setGroundElevationChecked(planeNew); + } + } // modified situations CAircraftSituation currentSituation(oldSituation); // also sets ground elevation if available @@ -209,7 +216,8 @@ namespace BlackMisc log.interpolationSituations.push_back(oldSituation); // oldest at back } - return { oldSituation, newSituation, simulationTimeFraction, interpolatedTime }; + m_interpolant = { oldSituation, newSituation, simulationTimeFraction, interpolatedTime }; + return m_interpolant; } } // namespace } // namespace diff --git a/src/blackmisc/simulation/interpolatorlinear.h b/src/blackmisc/simulation/interpolatorlinear.h index e52159256..b9f3ca84c 100644 --- a/src/blackmisc/simulation/interpolatorlinear.h +++ b/src/blackmisc/simulation/interpolatorlinear.h @@ -13,7 +13,8 @@ #define BLACKMISC_SIMULATION_INTERPOLATORLINEAR_H #include "interpolator.h" -#include "blackmisc/simulation/interpolationlogger.h" +#include "interpolationlogger.h" +#include "interpolant.h" #include "blackmisc/aviation/aircraftsituation.h" #include "blackmisc/blackmiscexport.h" #include @@ -37,41 +38,37 @@ namespace BlackMisc CInterpolator(callsign, simEnvProvider, setupProvider, remoteAircraftProvider, logger) {} //! Linear function that performs the actual interpolation - class Interpolant + class BLACKMISC_EXPORT CInterpolant : public IInterpolant { public: //! Constructor //! @{ - Interpolant(const Aviation::CAircraftSituation &situation); - Interpolant(const Aviation::CAircraftSituation &situation1, const Aviation::CAircraftSituation &situation2, double timeFraction, qint64 interpolatedTime); + CInterpolant() {} + CInterpolant(const Aviation::CAircraftSituation &situation); + CInterpolant(const Aviation::CAircraftSituation &situation, const CInterpolatorPbh &pbh); + CInterpolant(const Aviation::CAircraftSituation &situation1, const Aviation::CAircraftSituation &situation2, double timeFraction, qint64 interpolatedTime); //! @} //! Perform the interpolation Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor) const; - //! Interpolator for pitch, bank, heading, groundspeed - const CInterpolatorPbh &pbh() const { return m_pbh; } - //! Old situation const Aviation::CAircraftSituation &getOldSituation() const { return m_oldSituation; } //! New situation const Aviation::CAircraftSituation &getNewSituation() const { return m_newSituation; } - //! "Real time" representing the interpolated situation - qint64 getInterpolatedTime() const { return m_interpolatedTime; } - private: - int m_situationsAvailable = 0; Aviation::CAircraftSituation m_oldSituation; Aviation::CAircraftSituation m_newSituation; double m_simulationTimeFraction = 0.0; //!< 0..1 - qint64 m_interpolatedTime = 0; //!< "Real time "of interpolated situation - const CInterpolatorPbh m_pbh; //!< pitch, bank, ground speed and heading }; //! Get the interpolant for the given time point - Interpolant getInterpolant(qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetupPerCallsign &setup, CInterpolationStatus &status, SituationLog &log); + CInterpolant getInterpolant(qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetupPerCallsign &setup, CInterpolationStatus &status, SituationLog &log); + + private: + CInterpolant m_interpolant; //!< current interpolant }; } // ns } // ns diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index a90527eb0..5ed12bcd9 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -145,7 +145,7 @@ namespace BlackMisc return true; } - CInterpolatorSpline::Interpolant CInterpolatorSpline::getInterpolant( + CInterpolatorSpline::CInterpolant CInterpolatorSpline::getInterpolant( qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetupPerCallsign &setup, CInterpolationStatus &status, @@ -161,14 +161,13 @@ namespace BlackMisc { // with the latest updates of T243 the order and the offsets are supposed to be correct // so even mixing fast/slow updates shall work - const CAircraftSituationList validSituations = this->remoteAircraftSituations(m_callsign); m_situationsLastModifiedUsed = lastModified; + const CAircraftSituationList validSituations = this->remoteAircraftSituationsAndChange(true); const bool fillStatus = this->fillSituationsArray(validSituations); if (!fillStatus) { return m_interpolant; } - m_situationChange = CAircraftSituationChange(validSituations, true, true); 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 @@ -201,7 +200,7 @@ namespace BlackMisc m_nextSampleAdjustedTime = m_s[2].getAdjustedMSecsSinceEpoch(); // latest m_prevSampleTime = m_s[1].getMSecsSinceEpoch(); m_nextSampleTime = m_s[2].getMSecsSinceEpoch(); // latest - m_interpolant = Interpolant(pa, m_s[2].getAltitudeUnit(), CInterpolatorPbh(m_s[1], m_s[2])); + m_interpolant = CInterpolant(pa, m_s[2].getAltitudeUnit(), CInterpolatorPbh(m_s[1], m_s[2])); Q_ASSERT_X(m_prevSampleAdjustedTime < m_nextSampleAdjustedTime, Q_FUNC_INFO, "Wrong time order"); } @@ -286,7 +285,14 @@ namespace BlackMisc return true; } - CAircraftSituation CInterpolatorSpline::Interpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation, bool interpolateGndFactor) const + CInterpolatorSpline::CInterpolant::CInterpolant(const CInterpolatorSpline::PosArray &pa, const CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh) : + m_pa(pa), m_altitudeUnit(altitudeUnit) + { + m_pbh = pbh; + m_situationsAvailable = pa.size(); + } + + CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation, bool interpolateGndFactor) const { const double t1 = m_pa.t[1]; const double t2 = m_pa.t[2]; @@ -323,7 +329,7 @@ namespace BlackMisc return newSituation; } - void CInterpolatorSpline::Interpolant::setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs) + void CInterpolatorSpline::CInterpolant::setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs) { m_currentTimeMsSinceEpoc = currentTimeMs; m_interpolatedTime = interpolatedTimeMs; diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h index 9abe3fe82..0be5ef027 100644 --- a/src/blackmisc/simulation/interpolatorspline.h +++ b/src/blackmisc/simulation/interpolatorspline.h @@ -12,8 +12,9 @@ #ifndef BLACKMISC_SIMULATION_INTERPOLATORSPLINE_H #define BLACKMISC_SIMULATION_INTERPOLATORSPLINE_H -#include "blackmisc/simulation/interpolator.h" -#include "blackmisc/simulation/interpolationlogger.h" +#include "interpolator.h" +#include "interpolationlogger.h" +#include "interpolant.h" #include "blackmisc/aviation/aircraftsituation.h" #include "blackmisc/blackmiscexport.h" #include @@ -44,49 +45,42 @@ namespace BlackMisc //! 3 coordinates for spline interpolation @{ std::array x, y, z, a, gnd, t, dx, dy, dz, da, dgnd; + + //! Array size + int size() const { return x.size(); } //! @} }; //! Cubic function that performs the actual interpolation - class BLACKMISC_EXPORT Interpolant + class BLACKMISC_EXPORT CInterpolant : public IInterpolant { public: //! Default - Interpolant() : m_pa(PosArray::zeroPosArray()) {} + CInterpolant() : m_pa(PosArray::zeroPosArray()) {} //! Constructor - Interpolant( - const PosArray &pa, const PhysicalQuantities::CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh) : - m_pa(pa), m_altitudeUnit(altitudeUnit), m_pbh(pbh) {} + CInterpolant(const PosArray &pa, const PhysicalQuantities::CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh); //! Perform the interpolation Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation, bool interpolateGndFactor) const; - //! Interpolator for pitch, bank, heading, groundspeed - const CInterpolatorPbh &pbh() const { return m_pbh; } - //! Old situation const Aviation::CAircraftSituation &getOldSituation() const { return pbh().getOldSituation(); } //! New situation const Aviation::CAircraftSituation &getNewSituation() const { return pbh().getNewSituation(); } - //! "Real time" representing the interpolated situation - qint64 getInterpolatedTime() const { return m_interpolatedTime; } - //! Set the time values void setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs); private: PosArray m_pa; //! current positions array, latest values last PhysicalQuantities::CLengthUnit m_altitudeUnit; - CInterpolatorPbh m_pbh; qint64 m_currentTimeMsSinceEpoc { -1 }; - qint64 m_interpolatedTime { -1 }; //!< represented "real time" at interpolated situation }; //! Strategy used by CInterpolator::getInterpolatedSituation - Interpolant getInterpolant(qint64 currentTimeMsSinceEpoc, + CInterpolant getInterpolant(qint64 currentTimeMsSinceEpoc, const CInterpolationAndRenderingSetupPerCallsign &setup, CInterpolationStatus &status, SituationLog &log); private: @@ -110,7 +104,7 @@ namespace BlackMisc qint64 m_prevSampleTime = 0; //!< previous sample "real time" qint64 m_nextSampleTime = 0; //!< next sample "real time" std::array m_s; //!< used situations - Interpolant m_interpolant; + CInterpolant m_interpolant; }; } // ns } // ns