mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-04-17 10:55:32 +08:00
refactor: Split position/altitude and groundfactor interpolation
This commit is contained in:
@@ -258,7 +258,13 @@ namespace BlackMisc::Simulation
|
|||||||
|
|
||||||
// use derived interpolant function
|
// use derived interpolant function
|
||||||
const bool interpolateGndFlag = pbh.getEndSituation().hasGroundDetailsForGndInterpolation() && pbh.getStartSituation().hasGroundDetailsForGndInterpolation();
|
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 (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
|
// if we get here and the vector is invalid it means we haven't handled it correctly in one of the interpolators
|
||||||
|
|||||||
@@ -45,7 +45,7 @@ namespace BlackMisc::Simulation
|
|||||||
void CInterpolatorLinear::anchor()
|
void CInterpolatorLinear::anchor()
|
||||||
{}
|
{}
|
||||||
|
|
||||||
CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const
|
CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation) const
|
||||||
{
|
{
|
||||||
const std::array<double, 3> startVec(m_startSituation.getPosition().normalVectorDouble());
|
const std::array<double, 3> startVec(m_startSituation.getPosition().normalVectorDouble());
|
||||||
const std::array<double, 3> endVec(m_endSituation.getPosition().normalVectorDouble());
|
const std::array<double, 3> endVec(m_endSituation.getPosition().normalVectorDouble());
|
||||||
@@ -82,26 +82,28 @@ namespace BlackMisc::Simulation
|
|||||||
interpolatedSituation.setAltitude(altitude);
|
interpolatedSituation.setAltitude(altitude);
|
||||||
interpolatedSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
|
interpolatedSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
|
||||||
|
|
||||||
if (interpolateGndFactor)
|
return interpolatedSituation;
|
||||||
|
}
|
||||||
|
|
||||||
|
Aviation::COnGroundInfo CInterpolatorLinear::CInterpolant::interpolateGroundFactor() const
|
||||||
{
|
{
|
||||||
const double startGroundFactor = m_startSituation.getOnGroundInfo().getGroundFactor();
|
const double startGroundFactor = m_startSituation.getOnGroundInfo().getGroundFactor();
|
||||||
const double endGroundFactor = m_endSituation.getOnGroundInfo().getGroundFactor();
|
const double endGroundFactor = m_endSituation.getOnGroundInfo().getGroundFactor();
|
||||||
if (CAircraftSituation::isGfEqualAirborne(startGroundFactor, endGroundFactor))
|
if (CAircraftSituation::isGfEqualAirborne(startGroundFactor, endGroundFactor))
|
||||||
{
|
{
|
||||||
interpolatedSituation.setOnGroundInfo({ COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation });
|
return { COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation };
|
||||||
}
|
}
|
||||||
else if (CAircraftSituation::isGfEqualOnGround(startGroundFactor, endGroundFactor))
|
else if (CAircraftSituation::isGfEqualOnGround(startGroundFactor, endGroundFactor))
|
||||||
{
|
{
|
||||||
interpolatedSituation.setOnGroundInfo({ COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation });
|
return { COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation };
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
|
const double tf = clampValidTimeFraction(m_simulationTimeFraction);
|
||||||
const double interpolatedGroundFactor = (endGroundFactor - startGroundFactor) * tf + startGroundFactor;
|
const double interpolatedGroundFactor = (endGroundFactor - startGroundFactor) * tf + startGroundFactor;
|
||||||
interpolatedSituation.setOnGroundInfo(COnGroundInfo(interpolatedGroundFactor));
|
return COnGroundInfo(interpolatedGroundFactor);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return interpolatedSituation;
|
|
||||||
}
|
|
||||||
|
|
||||||
CInterpolatorLinear::CInterpolant CInterpolatorLinear::getInterpolant(SituationLog &log)
|
CInterpolatorLinear::CInterpolant CInterpolatorLinear::getInterpolant(SituationLog &log)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -49,9 +49,11 @@ namespace BlackMisc
|
|||||||
|
|
||||||
//! Perform the interpolation
|
//! Perform the interpolation
|
||||||
//! \param situation situation used as a base for interpolation. Contains for example the already interpolated PBH.
|
//! \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 and updated timestamp
|
||||||
//! \return \p situation with interpolated position and altitude, updated timestamp and possibly interpolated GND factor
|
Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation) const;
|
||||||
Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation, bool interpolateGndFactor) const;
|
|
||||||
|
//! Interpolate the ground information/factor
|
||||||
|
Aviation::COnGroundInfo interpolateGroundFactor() const;
|
||||||
|
|
||||||
//! Start situation
|
//! Start situation
|
||||||
const Aviation::CAircraftSituation &getStartSituation() const { return m_startSituation; }
|
const Aviation::CAircraftSituation &getStartSituation() const { return m_startSituation; }
|
||||||
|
|||||||
@@ -315,7 +315,7 @@ namespace BlackMisc::Simulation
|
|||||||
m_situationsAvailable = pa.size();
|
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 t1 = m_pa.t[1];
|
||||||
const double t2 = m_pa.t[2]; // latest (adjusted)
|
const double t2 = m_pa.t[2]; // latest (adjusted)
|
||||||
@@ -361,27 +361,33 @@ namespace BlackMisc::Simulation
|
|||||||
newSituation.setAltitude(alt);
|
newSituation.setAltitude(alt);
|
||||||
newSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
|
newSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
|
||||||
|
|
||||||
if (interpolateGndFactor)
|
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 gnd1 = m_pa.gnd[1];
|
||||||
const double gnd2 = m_pa.gnd[2]; // latest
|
const double gnd2 = m_pa.gnd[2]; // latest
|
||||||
|
|
||||||
if (CAircraftSituation::isGfEqualAirborne(gnd1, gnd2))
|
if (CAircraftSituation::isGfEqualAirborne(gnd1, gnd2))
|
||||||
{
|
{
|
||||||
newSituation.setOnGroundInfo({ COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation });
|
return { COnGroundInfo::NotOnGround, COnGroundInfo::OnGroundByInterpolation };
|
||||||
}
|
}
|
||||||
else if (CAircraftSituation::isGfEqualOnGround(gnd1, gnd2))
|
else if (CAircraftSituation::isGfEqualOnGround(gnd1, gnd2))
|
||||||
{
|
{
|
||||||
newSituation.setOnGroundInfo({ COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation });
|
return { COnGroundInfo::OnGround, COnGroundInfo::OnGroundByInterpolation };
|
||||||
}
|
}
|
||||||
else
|
else
|
||||||
{
|
{
|
||||||
const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]);
|
const double newGnd = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, gnd1, gnd2, m_pa.dgnd[1], m_pa.dgnd[2]);
|
||||||
newSituation.setOnGroundInfo(COnGroundInfo(newGnd));
|
return COnGroundInfo(newGnd);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
return newSituation;
|
|
||||||
}
|
|
||||||
|
|
||||||
void CInterpolatorSpline::CInterpolant::setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs)
|
void CInterpolatorSpline::CInterpolant::setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs)
|
||||||
{
|
{
|
||||||
|
|||||||
@@ -65,9 +65,11 @@ namespace BlackMisc::Simulation
|
|||||||
|
|
||||||
//! Perform the interpolation
|
//! Perform the interpolation
|
||||||
//! \param situation situation used as a base for interpolation. Contains for example the already interpolated PBH.
|
//! \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 and updated timestamp
|
||||||
//! \return \p situation with interpolated position and altitude, updated timestamp and possibly interpolated GND factor
|
Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation) const;
|
||||||
Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation, bool interpolateGndFactor) const;
|
|
||||||
|
//! Interpolate the ground information/factor
|
||||||
|
Aviation::COnGroundInfo interpolateGroundFactor() const;
|
||||||
|
|
||||||
//! Set the time values
|
//! Set the time values
|
||||||
void setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs);
|
void setTimes(qint64 currentTimeMs, double timeFraction, qint64 interpolatedTimeMs);
|
||||||
|
|||||||
Reference in New Issue
Block a user