diff --git a/src/blackcore/fsd/fsdclient.cpp b/src/blackcore/fsd/fsdclient.cpp index b01497cbb..5360c5bb9 100644 --- a/src/blackcore/fsd/fsdclient.cpp +++ b/src/blackcore/fsd/fsdclient.cpp @@ -1317,10 +1317,9 @@ namespace BlackCore::Fsd CAngle(dataUpdate.m_pitch, CAngleUnit::deg()), CAngle(dataUpdate.m_bank, CAngleUnit::deg())); - // not used - //situation.setVelocity(CAircraftVelocity( - // dataUpdate.m_xVelocity, dataUpdate.m_yVelocity, dataUpdate.m_zVelocity, CSpeedUnit::m_s(), - // dataUpdate.m_pitchRadPerSec, dataUpdate.m_bankRadPerSec, dataUpdate.m_headingRadPerSec, CAngleUnit::rad(), CTimeUnit::s())); + situation.setVelocity(CAircraftVelocity( + dataUpdate.m_xVelocity, dataUpdate.m_yVelocity, dataUpdate.m_zVelocity, CSpeedUnit::m_s(), + dataUpdate.m_pitchRadPerSec, dataUpdate.m_bankRadPerSec, dataUpdate.m_headingRadPerSec, CAngleUnit::rad(), CTimeUnit::s())); // Ref T297, default offset time situation.setCurrentUtcTime(); diff --git a/src/blackmisc/aviation/aircraftsituation.cpp b/src/blackmisc/aviation/aircraftsituation.cpp index 388f8dc6a..b3e01265a 100644 --- a/src/blackmisc/aviation/aircraftsituation.cpp +++ b/src/blackmisc/aviation/aircraftsituation.cpp @@ -928,6 +928,43 @@ namespace BlackMisc::Aviation ); } + CAircraftSituation CAircraftSituation::extrapolate(int ms) + { + auto copy = *this; + const double time = CAircraftVelocity::c_timeUnit.convertFrom(ms, CTimeUnit::ms()); + copy.m_position.adjust( + { m_velocity.getVelocityZ(CSpeedUnit::m_s()) * time, CLengthUnit::m() }, + { m_velocity.getVelocityX(CSpeedUnit::m_s()) * time, CLengthUnit::m() }, + { m_velocity.getVelocityY(CSpeedUnit::m_s()) * time, CLengthUnit::m() }); + copy.m_pitch.addValueSameUnit(time * m_velocity.getHeadingVelocity(m_pitch.getUnit(), CAircraftVelocity::c_timeUnit)); + copy.m_bank.addValueSameUnit(time * m_velocity.getHeadingVelocity(m_bank.getUnit(), CAircraftVelocity::c_timeUnit)); + copy.m_heading.addValueSameUnit(time * m_velocity.getHeadingVelocity(m_heading.getUnit(), CAircraftVelocity::c_timeUnit)); + return copy; + } + + CAircraftVelocity CAircraftSituation::calculateErrorVelocity(const CAircraftSituation& from, const CAircraftSituation& to, int ms, bool &o_ok) + { + if (ms == 0) { return {}; } + const CLength distance = from.calculateGreatCircleDistance(to); + const CAngle bearing = from.calculateBearing(to); + const double time = CAircraftVelocity::c_timeUnit.convertFrom(ms, CTimeUnit::ms()); + const double toAlt = to.m_position.geodeticHeight().value(CAircraftVelocity::c_xyzLengthUnit); + const double fromAlt = from.m_position.geodeticHeight().value(CAircraftVelocity::c_xyzLengthUnit); + o_ok = distance < CLength(100, CLengthUnit::m()) && std::abs(toAlt - fromAlt) < 100; + return + { + distance.value(CAircraftVelocity::c_xyzLengthUnit) * bearing.sin() / time, + (toAlt - fromAlt) / time, + distance.value(CAircraftVelocity::c_xyzLengthUnit) * bearing.cos() / time, + CAircraftVelocity::c_xyzSpeedUnit, + (to.m_pitch - from.m_pitch).value(CAircraftVelocity::c_pbhAngleUnit) / time, + (to.m_bank - from.m_bank).value(CAircraftVelocity::c_pbhAngleUnit) / time, + (to.m_heading - from.m_heading).value(CAircraftVelocity::c_pbhAngleUnit) / time, + CAircraftVelocity::c_pbhAngleUnit, + CAircraftVelocity::c_timeUnit + }; + } + bool CAircraftSituation::isMoving() const { const double gsKmh = this->getGroundSpeed().value(CSpeedUnit::km_h()); diff --git a/src/blackmisc/aviation/aircraftsituation.h b/src/blackmisc/aviation/aircraftsituation.h index ca3088a13..9d85e7d51 100644 --- a/src/blackmisc/aviation/aircraftsituation.h +++ b/src/blackmisc/aviation/aircraftsituation.h @@ -426,6 +426,12 @@ namespace BlackMisc //! Is velocity non-zero? bool hasVelocity() const { return m_hasVelocity; } + //! Return the situation adjusted for movement using the velocity plus error over time + CAircraftSituation extrapolate(int ms); + + //! Return the velocity needed to move from one position to another in the given time + static CAircraftVelocity calculateErrorVelocity(const CAircraftSituation &from, const CAircraftSituation &to, int ms, bool &o_ok); + //! Get ground speed const PhysicalQuantities::CSpeed &getGroundSpeed() const { return m_groundSpeed; } diff --git a/src/blackmisc/aviation/aircraftvelocity.cpp b/src/blackmisc/aviation/aircraftvelocity.cpp index 1e0892fc0..dd597546c 100644 --- a/src/blackmisc/aviation/aircraftvelocity.cpp +++ b/src/blackmisc/aviation/aircraftvelocity.cpp @@ -73,6 +73,28 @@ namespace BlackMisc::Aviation return c_timeUnit.convertFrom(angleUnit.convertFrom(m_heading, c_pbhAngleUnit), timeUnit); } + CAircraftVelocity& CAircraftVelocity::operator+=(const CAircraftVelocity& other) + { + m_x += other.m_x; + m_y += other.m_y; + m_z += other.m_z; + m_pitch += other.m_pitch; + m_roll += other.m_roll; + m_heading += other.m_heading; + return *this; + } + + CAircraftVelocity& CAircraftVelocity::operator-=(const CAircraftVelocity& other) + { + m_x -= other.m_x; + m_y -= other.m_y; + m_z -= other.m_z; + m_pitch -= other.m_pitch; + m_roll -= other.m_roll; + m_heading -= other.m_heading; + return *this; + } + QString CAircraftVelocity::convertToQString(bool i18n) const { return u"Velocity: " % QStringLiteral("%1 %2 %3 ").arg(m_x).arg(m_y).arg(m_z) % c_xyzSpeedUnit.convertToQString(i18n) % diff --git a/src/blackmisc/aviation/aircraftvelocity.h b/src/blackmisc/aviation/aircraftvelocity.h index 447ddaf1f..66c40dbdc 100644 --- a/src/blackmisc/aviation/aircraftvelocity.h +++ b/src/blackmisc/aviation/aircraftvelocity.h @@ -61,6 +61,14 @@ namespace BlackMisc::Aviation double getHeadingVelocity(PhysicalQuantities::CAngleUnit angleUnit, PhysicalQuantities::CTimeUnit timeUnit) const; //! @} + //! Arithmetic operator + //! @{ + friend CAircraftVelocity operator +(CAircraftVelocity a, const CAircraftVelocity &b) { return a += b; } + friend CAircraftVelocity operator -(CAircraftVelocity a, const CAircraftVelocity &b) { return a -= b; } + CAircraftVelocity &operator +=(const CAircraftVelocity &other); + CAircraftVelocity &operator -=(const CAircraftVelocity &other); + //! @} + //! \copydoc Mixin::String::toQString QString convertToQString(bool i18n = false) const; diff --git a/src/blackmisc/geo/coordinategeodetic.cpp b/src/blackmisc/geo/coordinategeodetic.cpp index c56c498fd..a33148750 100644 --- a/src/blackmisc/geo/coordinategeodetic.cpp +++ b/src/blackmisc/geo/coordinategeodetic.cpp @@ -20,6 +20,9 @@ using namespace BlackMisc::Math; BLACK_DEFINE_VALUEOBJECT_MIXINS(BlackMisc::Geo, CCoordinateGeodetic) +template +constexpr static T c_earthRadiusMeters = static_cast(6371000.8); + namespace BlackMisc::Geo { ICoordinateGeodetic::~ICoordinateGeodetic() @@ -47,14 +50,13 @@ namespace BlackMisc::Geo { if (coordinate1.isNull() || coordinate2.isNull()) { return CLength::null(); } // if (coordinate1.equalNormalVectorDouble(coordinate2)) { return CLength(0, CLengthUnit::defaultUnit()); } - constexpr float earthRadiusMeters = 6371000.8f; const QVector3D v1 = coordinate1.normalVector(); const QVector3D v2 = coordinate2.normalVector(); Q_ASSERT_X(std::isfinite(v1.x()) && std::isfinite(v1.y()) && std::isfinite(v1.z()), Q_FUNC_INFO, "Distance calculation: v1 non-finite argument"); Q_ASSERT_X(std::isfinite(v2.x()) && std::isfinite(v2.y()) && std::isfinite(v2.z()), Q_FUNC_INFO, "Distance calculation: v2 non-finite argument"); - const float d = earthRadiusMeters * std::atan2(QVector3D::crossProduct(v1, v2).length(), QVector3D::dotProduct(v1, v2)); + const float d = c_earthRadiusMeters * std::atan2(QVector3D::crossProduct(v1, v2).length(), QVector3D::dotProduct(v1, v2)); BLACK_VERIFY_X(!std::isnan(d), Q_FUNC_INFO, "Distance calculation: NaN in result"); if (std::isnan(d)) @@ -395,6 +397,13 @@ namespace BlackMisc::Geo this->setGeodeticHeight(CAltitude::null()); } + void CCoordinateGeodetic::adjust(const PhysicalQuantities::CLength& dLat, const PhysicalQuantities::CLength& dLon, const PhysicalQuantities::CLength& dAlt) + { + setLatitude({ latitude().value(CAngleUnit::rad()) + dLat.value(CLengthUnit::m()) / c_earthRadiusMeters, CAngleUnit::rad() }); + setLongitude({ longitude().value(CAngleUnit::rad()) + dLon.value(CLengthUnit::m()) / c_earthRadiusMeters / latitude().cos(), CAngleUnit::rad() }); + setGeodeticHeight(geodeticHeight().withOffset(dAlt)); + } + void CCoordinateGeodetic::setNormalVector(const std::array &normalVector) { Q_ASSERT_X(normalVector.size() == 3, Q_FUNC_INFO, "Wrong vector size"); diff --git a/src/blackmisc/geo/coordinategeodetic.h b/src/blackmisc/geo/coordinategeodetic.h index 52551780b..dd238d588 100644 --- a/src/blackmisc/geo/coordinategeodetic.h +++ b/src/blackmisc/geo/coordinategeodetic.h @@ -304,6 +304,9 @@ namespace BlackMisc //! Set height to NULL void setGeodeticHeightToNull(); + //! Add small position adjustment + void adjust(const PhysicalQuantities::CLength &dLat, const PhysicalQuantities::CLength &dLon, const PhysicalQuantities::CLength &dAlt); + //! Set normal vector void setNormalVector(const QVector3D &normal) { m_x = static_cast(normal.x()); m_y = static_cast(normal.y()); m_z = static_cast(normal.z()); } diff --git a/src/blackmisc/simulation/interpolant.h b/src/blackmisc/simulation/interpolant.h index 4e22929dd..0ce2b4f32 100644 --- a/src/blackmisc/simulation/interpolant.h +++ b/src/blackmisc/simulation/interpolant.h @@ -12,6 +12,7 @@ #define BLACKMISC_SIMULATION_INTERPOLANT_H #include "blackmisc/simulation/interpolatorpbh.h" +#include "blackmisc/simulation/interpolantvelocity.h" namespace BlackMisc::Simulation { @@ -40,6 +41,15 @@ namespace BlackMisc::Simulation //! Set recalculated interpolant void setRecalculated(bool reCalculated) { m_recalculated = reCalculated; } + //! Get the velocity interpolant + const CInterpolantVelocity &getVelocity() const { return m_velocity; } + + //! To be used by the velocity interpolant + //! @{ + void setLatestSituation(const Aviation::CAircraftSituation &situation) { m_velocity.setLatestSituation(situation); } + void setCurrentTime(qint64 msSinceEpoch) { m_velocity.setCurrentTime(msSinceEpoch); } + //! @} + protected: //! Default ctor IInterpolant() {} @@ -48,11 +58,15 @@ namespace BlackMisc::Simulation IInterpolant(int situationsAvailable, const CInterpolatorPbh &pbh) : m_situationsAvailable(situationsAvailable), m_pbh(pbh) {} //! Constructor - IInterpolant(qint64 interpolatedTime, int situationsAvailable) : m_interpolatedTime(interpolatedTime), m_situationsAvailable(situationsAvailable) {} + IInterpolant(qint64 interpolatedTime, int situationsAvailable, const CInterpolantVelocity &velocity) : m_interpolatedTime(interpolatedTime), m_situationsAvailable(situationsAvailable), m_velocity(velocity) {} + + //! Constructor + IInterpolant(const CInterpolantVelocity &velocity) : m_velocity(velocity) {} qint64 m_interpolatedTime = -1; //!< "Real time "of interpolated situation int m_situationsAvailable = 0; //!< used situations CInterpolatorPbh m_pbh; //!< the used PBH interpolator + CInterpolantVelocity m_velocity;//!< the used velocity interpolant bool m_valid = true; //!< valid? bool m_recalculated = false; //!< recalculated interpolant }; diff --git a/src/blackmisc/simulation/interpolantvelocity.cpp b/src/blackmisc/simulation/interpolantvelocity.cpp new file mode 100644 index 000000000..28885e860 --- /dev/null +++ b/src/blackmisc/simulation/interpolantvelocity.cpp @@ -0,0 +1,65 @@ +/* Copyright (C) 2022 + * swift project Community / Contributors + * + * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level + * directory of this distribution. No part of swift project, including this file, may be copied, modified, propagated, + * or distributed except according to the terms contained in the LICENSE file. + */ + +//! \file + +#include "blackmisc/simulation/interpolantvelocity.h" +#include "blackmisc/logmessage.h" + +using namespace BlackMisc::Aviation; + +static constexpr int c_errorCorrectionPeriodMs = 2000; + +namespace BlackMisc::Simulation +{ + void CInterpolantVelocity::setLatestSituation(const CAircraftSituation& situation) + { + if (situation.hasVelocity()) + { + if (isReady()) + { + bool ok = true; + const CAircraftSituation extrapolated = extrapolate(situation.getMSecsSinceEpoch()); + const CAircraftVelocity error = CAircraftSituation::calculateErrorVelocity(extrapolated, situation, c_errorCorrectionPeriodMs, ok); + if (ok) + { + m_situation = extrapolated; + m_situation.setVelocity(situation.getVelocity() + error); + } + else + { + m_situation = situation; + + CLogMessage(this).debug(u"Error velocity exceeded threshold: %1") << error; + } + } + else + { + m_situation = situation; + } + } + else + { + m_situation.setNull(); + return; + } + } + + bool CInterpolantVelocity::isReady() const + { + return !m_situation.isNull(); + } + + CAircraftSituation CInterpolantVelocity::extrapolate(qint64 time) + { + Q_ASSERT(isReady()); + Q_ASSERT(m_situation.hasVelocity()); + const qint64 deltaTime = (time < 0 ? m_time : time) - m_situation.getMSecsSinceEpoch(); + return m_situation.extrapolate(static_cast(deltaTime)); + } +} diff --git a/src/blackmisc/simulation/interpolantvelocity.h b/src/blackmisc/simulation/interpolantvelocity.h new file mode 100644 index 000000000..d32d48c0c --- /dev/null +++ b/src/blackmisc/simulation/interpolantvelocity.h @@ -0,0 +1,40 @@ +/* Copyright (C) 2022 + * swift project Community / Contributors + * + * This file is part of swift project. It is subject to the license terms in the LICENSE file found in the top-level + * directory of this distribution. No part of swift project, including this file, may be copied, modified, propagated, + * or distributed except according to the terms contained in the LICENSE file. + */ + +//! \file + +#ifndef BLACKMISC_SIMULATION_INTERPOLANTVELOCITY_H +#define BLACKMISC_SIMULATION_INTERPOLANTVELOCITY_H + +#include "blackmisc/aviation/aircraftsituation.h" + +namespace BlackMisc::Simulation +{ + //! Interpolant that uses velocity from visual position updates + class CInterpolantVelocity + { + public: + //! Set the time to be used for extrapolation + void setCurrentTime(qint64 msSinceEpoch) { m_time = msSinceEpoch; } + + //! Set the situation to use for extrapolation + void setLatestSituation(const Aviation::CAircraftSituation& situation); + + //! Is it ready to call extrapolate? + bool isReady() const; + + //! Extrapolate situation at the given time or the one passed to setCurrentTime + Aviation::CAircraftSituation extrapolate(qint64 time = -1); + + private: + qint64 m_time = 0; + Aviation::CAircraftSituation m_situation; + }; +} + +#endif diff --git a/src/blackmisc/simulation/interpolator.cpp b/src/blackmisc/simulation/interpolator.cpp index c42a8f04f..aa6bfe6fd 100644 --- a/src/blackmisc/simulation/interpolator.cpp +++ b/src/blackmisc/simulation/interpolator.cpp @@ -252,7 +252,7 @@ namespace BlackMisc::Simulation // interpolant as function of derived class // CInterpolatorLinear::Interpolant or CInterpolatorSpline::Interpolant SituationLog log; - const auto interpolant = derived()->getInterpolant(log); + auto interpolant = derived()->getInterpolant(log); const bool isValidInterpolant = interpolant.isValid(); CAircraftSituation currentSituation = m_lastSituation; @@ -712,11 +712,16 @@ namespace BlackMisc::Simulation m_currentInterpolationStatus.reset(); m_currentPartsStatus.reset(); m_currentSetup = setup; + derived()->updateInterpolantTime(); if (changedSituations) { m_situationsLastModified = lastModifed; m_currentSituations = this->remoteAircraftSituationsAndChange(setup); // only update when needed + if (!m_currentSituations.isEmpty()) + { + derived()->setLatestSituation(m_currentSituations.front()); + } } if (!m_model.hasCG() || slowUpdateStep) diff --git a/src/blackmisc/simulation/interpolatorlinear.cpp b/src/blackmisc/simulation/interpolatorlinear.cpp index 0f316a18d..f744c4efb 100644 --- a/src/blackmisc/simulation/interpolatorlinear.cpp +++ b/src/blackmisc/simulation/interpolatorlinear.cpp @@ -44,8 +44,8 @@ namespace BlackMisc::Simulation m_oldSituation(oldSituation) { } - CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime) : - IInterpolant(interpolatedTime, 2), + CInterpolatorLinear::CInterpolant::CInterpolant(const CAircraftSituation &oldSituation, const CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime, const CInterpolantVelocity &velocity) : + IInterpolant(interpolatedTime, 2, velocity), m_oldSituation(oldSituation), m_newSituation(newSituation), m_simulationTimeFraction(timeFraction) { @@ -55,8 +55,13 @@ namespace BlackMisc::Simulation void CInterpolatorLinear::anchor() { } - CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const + CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) { + if (m_velocity.isReady()) + { + return m_velocity.extrapolate(); + } + const std::array oldVec(m_oldSituation.getPosition().normalVectorDouble()); const std::array newVec(m_newSituation.getPosition().normalVectorDouble()); @@ -227,7 +232,7 @@ namespace BlackMisc::Simulation log.interpolantRecalc = recalculate; } - m_interpolant = { oldSituation, newSituation, simulationTimeFraction, interpolatedTime }; + m_interpolant = { oldSituation, newSituation, simulationTimeFraction, interpolatedTime, m_interpolant.getVelocity() }; m_interpolant.setRecalculated(recalculate); return m_interpolant; diff --git a/src/blackmisc/simulation/interpolatorlinear.h b/src/blackmisc/simulation/interpolatorlinear.h index ee3b76e20..5b2a64024 100644 --- a/src/blackmisc/simulation/interpolatorlinear.h +++ b/src/blackmisc/simulation/interpolatorlinear.h @@ -14,6 +14,7 @@ #include "blackmisc/simulation/interpolator.h" #include "blackmisc/simulation/interpolationlogger.h" #include "blackmisc/simulation/interpolant.h" +#include "blackmisc/simulation/interpolantvelocity.h" #include "blackmisc/aviation/aircraftsituation.h" #include "blackmisc/blackmiscexport.h" #include @@ -47,11 +48,11 @@ namespace BlackMisc CInterpolant() {} CInterpolant(const Aviation::CAircraftSituation &oldSituation); CInterpolant(const Aviation::CAircraftSituation &oldSituation, const CInterpolatorPbh &pbh); - CInterpolant(const Aviation::CAircraftSituation &oldSituation, const Aviation::CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime); + CInterpolant(const Aviation::CAircraftSituation &oldSituation, const Aviation::CAircraftSituation &newSituation, double timeFraction, qint64 interpolatedTime, const CInterpolantVelocity &velocity); //! @} //! Perform the interpolation - Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor) const; + Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor); //! Old situation const Aviation::CAircraftSituation &getOldSituation() const { return m_oldSituation; } @@ -68,6 +69,12 @@ namespace BlackMisc //! Get the interpolant for the given time point CInterpolant getInterpolant(SituationLog &log); + //! To be used by the velocity interpolant + //! @{ + void setLatestSituation(const Aviation::CAircraftSituation &situation) { m_interpolant.setLatestSituation(situation); } + void updateInterpolantTime() { m_interpolant.setCurrentTime(m_currentTimeMsSinceEpoch); } + //! @} + private: CInterpolant m_interpolant; //!< current interpolant }; diff --git a/src/blackmisc/simulation/interpolatorspline.cpp b/src/blackmisc/simulation/interpolatorspline.cpp index 37b124b31..d6c8bec39 100644 --- a/src/blackmisc/simulation/interpolatorspline.cpp +++ b/src/blackmisc/simulation/interpolatorspline.cpp @@ -232,7 +232,7 @@ namespace BlackMisc::Simulation m_nextSampleAdjustedTime = m_s[2].getAdjustedMSecsSinceEpoch(); // latest m_prevSampleTime = m_s[1].getMSecsSinceEpoch(); // last interpolated situation normally m_nextSampleTime = m_s[2].getMSecsSinceEpoch(); // latest - m_interpolant = CInterpolant(pa, altUnit, CInterpolatorPbh(m_s[1], m_s[2])); // older, newer + m_interpolant = CInterpolant(pa, altUnit, CInterpolatorPbh(m_s[1], m_s[2]), m_interpolant.getVelocity()); // older, newer Q_ASSERT_X(m_prevSampleAdjustedTime < m_nextSampleAdjustedTime, Q_FUNC_INFO, "Wrong time order"); } @@ -315,15 +315,21 @@ namespace BlackMisc::Simulation return false; } - CInterpolatorSpline::CInterpolant::CInterpolant(const CInterpolatorSpline::PosArray &pa, const CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh) : + CInterpolatorSpline::CInterpolant::CInterpolant(const CInterpolatorSpline::PosArray &pa, const CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh, const CInterpolantVelocity &velocity) : + IInterpolant(velocity), m_pa(pa), m_altitudeUnit(altitudeUnit) { m_pbh = pbh; m_situationsAvailable = pa.size(); } - CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation, bool interpolateGndFactor) const + CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation, bool interpolateGndFactor) { + if (m_velocity.isReady()) + { + return m_velocity.extrapolate(); + } + const double t1 = m_pa.t[1]; const double t2 = m_pa.t[2]; // latest (adjusted) diff --git a/src/blackmisc/simulation/interpolatorspline.h b/src/blackmisc/simulation/interpolatorspline.h index f6a090a3c..a08160c5d 100644 --- a/src/blackmisc/simulation/interpolatorspline.h +++ b/src/blackmisc/simulation/interpolatorspline.h @@ -59,10 +59,10 @@ namespace BlackMisc::Simulation CInterpolant() : m_pa(PosArray::zeroPosArray()) {} //! Constructor - CInterpolant(const PosArray &pa, const PhysicalQuantities::CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh); + CInterpolant(const PosArray &pa, const PhysicalQuantities::CLengthUnit &altitudeUnit, const CInterpolatorPbh &pbh, const CInterpolantVelocity &velocity); //! Perform the interpolation - Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation, bool interpolateGndFactor) const; + Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation, bool interpolateGndFactor); //! Old situation const Aviation::CAircraftSituation &getOldSituation() const { return pbh().getOldSituation(); } @@ -85,6 +85,12 @@ namespace BlackMisc::Simulation //! Strategy used by CInterpolator::getInterpolatedSituation CInterpolant getInterpolant(SituationLog &log); + //! To be used by the velocity interpolant + //! @{ + void setLatestSituation(const Aviation::CAircraftSituation &situation) { m_interpolant.setLatestSituation(situation); } + void updateInterpolantTime() { m_interpolant.setCurrentTime(m_currentTimeMsSinceEpoch); } + //! @} + private: //! Update the elevations used in CInterpolatorSpline::m_s bool updateElevations(bool canSkip); diff --git a/src/blackmisc/simulation/remoteaircraftprovider.cpp b/src/blackmisc/simulation/remoteaircraftprovider.cpp index 802619122..9d97b65ee 100644 --- a/src/blackmisc/simulation/remoteaircraftprovider.cpp +++ b/src/blackmisc/simulation/remoteaircraftprovider.cpp @@ -280,19 +280,19 @@ namespace BlackMisc::Simulation { const qint64 now = QDateTime::currentMSecsSinceEpoch(); QWriteLocker lock(&m_lockSituations); + CAircraftSituationList &newSituationsList = m_situationsByCallsign[cs]; + if (!situationCorrected.hasVelocity() && !newSituationsList.isEmpty() && newSituationsList.front().hasVelocity()) + { + return situationCorrected; + } m_situationsAdded++; m_situationsLastModified[cs] = now; - CAircraftSituationList &newSituationsList = m_situationsByCallsign[cs]; newSituationsList.setAdjustedSortHint(CAircraftSituationList::AdjustedTimestampLatestFirst); const int situations = newSituationsList.size(); if (situations < 1) { newSituationsList.prefillLatestAdjustedFirst(situationCorrected, IRemoteAircraftProvider::MaxSituationsPerCallsign); } - else if (!situationCorrected.hasVelocity() && newSituationsList.front().hasVelocity()) - { - return situationCorrected; - } else { // newSituationsList.push_frontKeepLatestFirstIgnoreOverlapping(situationCorrected, true, IRemoteAircraftProvider::MaxSituationsPerCallsign);