This commit is contained in:
Mat Sutcliffe
2022-02-17 22:27:00 +00:00
parent 684b2b1068
commit 4be8499eab
16 changed files with 256 additions and 24 deletions

View File

@@ -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();

View File

@@ -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());

View File

@@ -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; }

View File

@@ -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) %

View File

@@ -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;

View File

@@ -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");

View File

@@ -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()); }

View File

@@ -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
};

View 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));
}
}

View 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

View File

@@ -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)

View File

@@ -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;

View File

@@ -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
};

View File

@@ -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 &currentSituation, bool interpolateGndFactor) const
CAircraftSituation CInterpolatorSpline::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &currentSituation, 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)

View File

@@ -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 &currentSituation, bool interpolateGndFactor) const;
Aviation::CAircraftSituation interpolatePositionAndAltitude(const Aviation::CAircraftSituation &currentSituation, 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);

View File

@@ -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);