mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-03-30 20:15:35 +08:00
WIP
This commit is contained in:
@@ -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();
|
||||
|
||||
@@ -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());
|
||||
|
||||
@@ -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; }
|
||||
|
||||
|
||||
@@ -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) %
|
||||
|
||||
@@ -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;
|
||||
|
||||
|
||||
@@ -20,6 +20,9 @@ using namespace BlackMisc::Math;
|
||||
|
||||
BLACK_DEFINE_VALUEOBJECT_MIXINS(BlackMisc::Geo, CCoordinateGeodetic)
|
||||
|
||||
template <typename T>
|
||||
constexpr static T c_earthRadiusMeters = static_cast<T>(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<float> * 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<double>, CAngleUnit::rad() });
|
||||
setLongitude({ longitude().value(CAngleUnit::rad()) + dLon.value(CLengthUnit::m()) / c_earthRadiusMeters<double> / latitude().cos(), CAngleUnit::rad() });
|
||||
setGeodeticHeight(geodeticHeight().withOffset(dAlt));
|
||||
}
|
||||
|
||||
void CCoordinateGeodetic::setNormalVector(const std::array<double, 3> &normalVector)
|
||||
{
|
||||
Q_ASSERT_X(normalVector.size() == 3, Q_FUNC_INFO, "Wrong vector size");
|
||||
|
||||
@@ -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<double>(normal.x()); m_y = static_cast<double>(normal.y()); m_z = static_cast<double>(normal.z()); }
|
||||
|
||||
|
||||
@@ -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
|
||||
};
|
||||
|
||||
65
src/blackmisc/simulation/interpolantvelocity.cpp
Normal file
65
src/blackmisc/simulation/interpolantvelocity.cpp
Normal file
@@ -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<int>(deltaTime));
|
||||
}
|
||||
}
|
||||
40
src/blackmisc/simulation/interpolantvelocity.h
Normal file
40
src/blackmisc/simulation/interpolantvelocity.h
Normal file
@@ -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
|
||||
@@ -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)
|
||||
|
||||
@@ -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<double, 3> oldVec(m_oldSituation.getPosition().normalVectorDouble());
|
||||
const std::array<double, 3> 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;
|
||||
|
||||
@@ -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 <QString>
|
||||
@@ -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
|
||||
};
|
||||
|
||||
@@ -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)
|
||||
|
||||
|
||||
@@ -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);
|
||||
|
||||
@@ -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);
|
||||
|
||||
Reference in New Issue
Block a user