mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-04-07 11:05:33 +08:00
refactor: Simplify interpolatePositionAndAltitude method
This commit is contained in:
@@ -257,9 +257,12 @@ namespace BlackMisc::Simulation
|
|||||||
currentSituation.setGroundSpeed(pbh.getGroundSpeed());
|
currentSituation.setGroundSpeed(pbh.getGroundSpeed());
|
||||||
|
|
||||||
// use derived interpolant function
|
// use derived interpolant function
|
||||||
const bool interpolateGndFlag = pbh.getEndSituation().hasGroundDetailsForGndInterpolation() && pbh.getStartSituation().hasGroundDetailsForGndInterpolation();
|
const auto [interpolatedPosition, interpolatedAltitude] = interpolant.interpolatePositionAndAltitude();
|
||||||
currentSituation = interpolant.interpolatePositionAndAltitude(currentSituation);
|
currentSituation.setPosition(interpolatedPosition);
|
||||||
|
currentSituation.setAltitude(interpolatedAltitude);
|
||||||
|
currentSituation.setMSecsSinceEpoch(interpolant.getInterpolatedTime());
|
||||||
|
|
||||||
|
const bool interpolateGndFlag = pbh.getEndSituation().hasGroundDetailsForGndInterpolation() && pbh.getStartSituation().hasGroundDetailsForGndInterpolation();
|
||||||
if (interpolateGndFlag)
|
if (interpolateGndFlag)
|
||||||
{
|
{
|
||||||
currentSituation.setOnGroundInfo(interpolant.interpolateGroundFactor());
|
currentSituation.setOnGroundInfo(interpolant.interpolateGroundFactor());
|
||||||
|
|||||||
@@ -45,7 +45,7 @@ namespace BlackMisc::Simulation
|
|||||||
void CInterpolatorLinear::anchor()
|
void CInterpolatorLinear::anchor()
|
||||||
{}
|
{}
|
||||||
|
|
||||||
CAircraftSituation CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation) const
|
std::tuple<Geo::CCoordinateGeodetic, Aviation::CAltitude> CInterpolatorLinear::CInterpolant::interpolatePositionAndAltitude() 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());
|
||||||
@@ -77,12 +77,7 @@ namespace BlackMisc::Simulation
|
|||||||
const CAltitude altitude((newAlt - oldAlt) * tf + oldAlt,
|
const CAltitude altitude((newAlt - oldAlt) * tf + oldAlt,
|
||||||
oldAlt.getReferenceDatum());
|
oldAlt.getReferenceDatum());
|
||||||
|
|
||||||
CAircraftSituation interpolatedSituation(situation);
|
return { interpolatedPosition, altitude };
|
||||||
interpolatedSituation.setPosition(interpolatedPosition);
|
|
||||||
interpolatedSituation.setAltitude(altitude);
|
|
||||||
interpolatedSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
|
|
||||||
|
|
||||||
return interpolatedSituation;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Aviation::COnGroundInfo CInterpolatorLinear::CInterpolant::interpolateGroundFactor() const
|
Aviation::COnGroundInfo CInterpolatorLinear::CInterpolant::interpolateGroundFactor() const
|
||||||
|
|||||||
@@ -48,9 +48,8 @@ namespace BlackMisc
|
|||||||
//! @}
|
//! @}
|
||||||
|
|
||||||
//! Perform the interpolation
|
//! Perform the interpolation
|
||||||
//! \param situation situation used as a base for interpolation. Contains for example the already interpolated PBH.
|
//! \return interpolated position and altitude
|
||||||
//! \return \p situation with interpolated position and altitude and updated timestamp
|
std::tuple<Geo::CCoordinateGeodetic, Aviation::CAltitude> interpolatePositionAndAltitude() const;
|
||||||
Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &situation) const;
|
|
||||||
|
|
||||||
//! Interpolate the ground information/factor
|
//! Interpolate the ground information/factor
|
||||||
Aviation::COnGroundInfo interpolateGroundFactor() const;
|
Aviation::COnGroundInfo interpolateGroundFactor() const;
|
||||||
|
|||||||
@@ -315,7 +315,7 @@ namespace BlackMisc::Simulation
|
|||||||
m_situationsAvailable = pa.size();
|
m_situationsAvailable = pa.size();
|
||||||
}
|
}
|
||||||
|
|
||||||
CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation ¤tSituation) const
|
std::tuple<Geo::CCoordinateGeodetic, Aviation::CAltitude> CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude() 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)
|
||||||
@@ -327,7 +327,7 @@ namespace BlackMisc::Simulation
|
|||||||
BLACK_VERIFY_X(m_currentTimeMsSinceEpoc >= t1, Q_FUNC_INFO, "invalid timestamp t1");
|
BLACK_VERIFY_X(m_currentTimeMsSinceEpoc >= t1, Q_FUNC_INFO, "invalid timestamp t1");
|
||||||
BLACK_VERIFY_X(m_currentTimeMsSinceEpoc < t2, Q_FUNC_INFO, "invalid timestamp t2"); // t1==t2 results in div/0
|
BLACK_VERIFY_X(m_currentTimeMsSinceEpoc < t2, Q_FUNC_INFO, "invalid timestamp t2"); // t1==t2 results in div/0
|
||||||
}
|
}
|
||||||
if (!valid) { return CAircraftSituation::null(); }
|
if (!valid) { return { {}, {} }; }
|
||||||
|
|
||||||
const double newX = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.x[1], m_pa.x[2], m_pa.dx[1], m_pa.dx[2]);
|
const double newX = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.x[1], m_pa.x[2], m_pa.dx[1], m_pa.dx[2]);
|
||||||
const double newY = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.y[1], m_pa.y[2], m_pa.dy[1], m_pa.dy[2]);
|
const double newY = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.y[1], m_pa.y[2], m_pa.dy[1], m_pa.dy[2]);
|
||||||
@@ -340,9 +340,8 @@ namespace BlackMisc::Simulation
|
|||||||
BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.y), Q_FUNC_INFO, "invalid Y"); // all y values
|
BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.y), Q_FUNC_INFO, "invalid Y"); // all y values
|
||||||
BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.z), Q_FUNC_INFO, "invalid Z"); // all z values
|
BLACK_VERIFY_X(CAircraftSituation::isValidVector(m_pa.z), Q_FUNC_INFO, "invalid Z"); // all z values
|
||||||
}
|
}
|
||||||
if (!valid) { return CAircraftSituation::null(); }
|
if (!valid) { return { {}, {} }; }
|
||||||
|
|
||||||
CAircraftSituation newSituation(currentSituation);
|
|
||||||
const std::array<double, 3> normalVector = { { newX, newY, newZ } };
|
const std::array<double, 3> normalVector = { { newX, newY, newZ } };
|
||||||
const CCoordinateGeodetic currentPosition(normalVector);
|
const CCoordinateGeodetic currentPosition(normalVector);
|
||||||
|
|
||||||
@@ -350,18 +349,14 @@ namespace BlackMisc::Simulation
|
|||||||
if (!valid && CBuildConfig::isLocalDeveloperDebugBuild())
|
if (!valid && CBuildConfig::isLocalDeveloperDebugBuild())
|
||||||
{
|
{
|
||||||
BLACK_VERIFY_X(valid, Q_FUNC_INFO, "invalid vector");
|
BLACK_VERIFY_X(valid, Q_FUNC_INFO, "invalid vector");
|
||||||
CLogMessage(this).warning(u"Invalid vector for '%1' v: %2 %3 %4") << currentSituation.getCallsign().asString() << normalVector[0] << normalVector[1] << normalVector[2];
|
CLogMessage(this).warning(u"Invalid vector v: %2 %3 %4") << normalVector[0] << normalVector[1] << normalVector[2];
|
||||||
}
|
}
|
||||||
if (!valid) { return CAircraftSituation::null(); }
|
if (!valid) { return { {}, {} }; }
|
||||||
|
|
||||||
const double newA = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.a[1], m_pa.a[2], m_pa.da[1], m_pa.da[2]);
|
const double newA = evalSplineInterval(m_currentTimeMsSinceEpoc, t1, t2, m_pa.a[1], m_pa.a[2], m_pa.da[1], m_pa.da[2]);
|
||||||
const CAltitude alt(newA, m_altitudeUnit);
|
const CAltitude alt(newA, m_altitudeUnit);
|
||||||
|
|
||||||
newSituation.setPosition(currentPosition);
|
return { currentPosition, alt };
|
||||||
newSituation.setAltitude(alt);
|
|
||||||
newSituation.setMSecsSinceEpoch(this->getInterpolatedTime());
|
|
||||||
|
|
||||||
return newSituation;
|
|
||||||
}
|
}
|
||||||
|
|
||||||
Aviation::COnGroundInfo CInterpolatorSpline::CInterpolant::interpolateGroundFactor() const
|
Aviation::COnGroundInfo CInterpolatorSpline::CInterpolant::interpolateGroundFactor() const
|
||||||
|
|||||||
@@ -66,7 +66,7 @@ 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.
|
||||||
//! \return \p situation with interpolated position and altitude and updated timestamp
|
//! \return \p situation with interpolated position and altitude and updated timestamp
|
||||||
Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation ¤tSituation) const;
|
std::tuple<Geo::CCoordinateGeodetic, Aviation::CAltitude> interpolatePositionAndAltitude() const;
|
||||||
|
|
||||||
//! Interpolate the ground information/factor
|
//! Interpolate the ground information/factor
|
||||||
Aviation::COnGroundInfo interpolateGroundFactor() const;
|
Aviation::COnGroundInfo interpolateGroundFactor() const;
|
||||||
|
|||||||
Reference in New Issue
Block a user