mirror of
https://github.com/swift-project/pilotclient.git
synced 2026-03-23 23:45:35 +08:00
217 lines
10 KiB
C++
217 lines
10 KiB
C++
/* 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.
|
|
*/
|
|
|
|
#include "blackmisc/simulation/interpolatorvelocity.h"
|
|
#include "blackmisc/simulation/interpolatorfunctions.h"
|
|
#include "blackmisc/aviation/aircraftsituationlist.h"
|
|
#include "blackmisc/aviation/altitude.h"
|
|
#include "blackmisc/geo/coordinategeodetic.h"
|
|
#include "blackmisc/pq/length.h"
|
|
#include "blackmisc/pq/physicalquantity.h"
|
|
#include "blackmisc/logmessage.h"
|
|
#include "blackmisc/mixin/mixincompare.h"
|
|
#include "blackmisc/verify.h"
|
|
#include "blackmisc/range.h"
|
|
#include "blackmisc/sequence.h"
|
|
#include "blackmisc/statusmessage.h"
|
|
#include "blackconfig/buildconfig.h"
|
|
|
|
#include <QDateTime>
|
|
#include <QList>
|
|
#include <array>
|
|
|
|
using namespace BlackConfig;
|
|
using namespace BlackMisc::Aviation;
|
|
using namespace BlackMisc::Geo;
|
|
using namespace BlackMisc::Math;
|
|
using namespace BlackMisc::PhysicalQuantities;
|
|
using namespace BlackMisc::Simulation;
|
|
|
|
namespace BlackMisc::Simulation
|
|
{
|
|
CInterpolatorVelocity::CInterpolant::CInterpolant(const CAircraftSituation ¤tSituation) :
|
|
IInterpolant(1, CInterpolatorPbh(0, currentSituation, currentSituation)),
|
|
m_currentSitutation(currentSituation)
|
|
{
|
|
m_creationTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch();
|
|
m_currentTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch();
|
|
}
|
|
|
|
CInterpolatorVelocity::CInterpolant::CInterpolant(const CInterpolant &old, const CAircraftSituation ¤tSituation) :
|
|
IInterpolant(1, CInterpolatorPbh(0, currentSituation, currentSituation)),
|
|
m_currentSitutation(currentSituation)
|
|
{
|
|
m_creationTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch();
|
|
m_currentTimeMsSinceEpoch = currentSituation.getMSecsSinceEpoch();
|
|
|
|
// Delta to last shown position (correction vectors)
|
|
CAircraftSituation unusedSituation;
|
|
CAircraftSituation lastSitutation = old.interpolatePositionAndAltitude(unusedSituation, true);
|
|
m_altitudeDelta = lastSitutation.getAltitude().value(CLengthUnit::ft()) - currentSituation.getCorrectedAltitude().value(CLengthUnit::ft());
|
|
m_latDelta = lastSitutation.getPosition().latitude().value(CAngleUnit::deg()) - currentSituation.getPosition().latitude().value(CAngleUnit::deg());
|
|
m_lonDelta = lastSitutation.getPosition().longitude().value(CAngleUnit::deg()) - currentSituation.getPosition().longitude().value(CAngleUnit::deg());
|
|
|
|
auto correctTurnDirection = [](double delta) -> double
|
|
{
|
|
// Always take the shortest turning angle
|
|
if (delta > M_PI)
|
|
{
|
|
// Wrap 180 deg and turning to positiv direction to -180 deg
|
|
delta = - (2 * M_PI - delta);
|
|
}
|
|
else if (delta < -M_PI)
|
|
{
|
|
// Wrap -180 deg and turning to negative direction to 180 deg
|
|
delta = 2 * M_PI - std::abs(delta);
|
|
}
|
|
return delta;
|
|
};
|
|
|
|
m_bankDelta = lastSitutation.getBank().value(CAngleUnit::rad()) - currentSituation.getBank().value(CAngleUnit::rad());
|
|
m_bankDelta = correctTurnDirection(m_bankDelta);
|
|
|
|
m_headingDelta = lastSitutation.getHeading().value(CAngleUnit::rad()) - currentSituation.getHeading().value(CAngleUnit::rad());
|
|
m_headingDelta = correctTurnDirection(m_headingDelta);
|
|
|
|
m_pitchDelta = lastSitutation.getPitch().value(CAngleUnit::rad()) - currentSituation.getPitch().value(CAngleUnit::rad());
|
|
m_pitchDelta = correctTurnDirection(m_pitchDelta);
|
|
}
|
|
|
|
void CInterpolatorVelocity::anchor()
|
|
{ }
|
|
|
|
CAircraftSituation CInterpolatorVelocity::CInterpolant::interpolatePositionAndAltitude(const CAircraftSituation &situation, bool interpolateGndFactor) const
|
|
{
|
|
// Inspired by xPilot
|
|
// https://github.com/xpilot-project/xpilot/blob/97cf83efd650464b997dd3d5dee0ccde198cdf2c/plugin/src/NetworkAircraft.cpp
|
|
Q_ASSERT_X(m_currentTimeMsSinceEpoch >= m_creationTimeMsSinceEpoch, Q_FUNC_INFO, "Current time must be equal or greater than creation time");
|
|
|
|
constexpr qint64 maxExtrapolationMs = 7000;
|
|
qint64 diffMs = m_currentTimeMsSinceEpoch - m_creationTimeMsSinceEpoch;
|
|
if (diffMs > maxExtrapolationMs)
|
|
{
|
|
// Do not extrapolate more than maxExtrapolationMs (e.g. user timed out)
|
|
diffMs = maxExtrapolationMs;
|
|
}
|
|
|
|
// situation might include ground elevation already -> copy
|
|
CAircraftSituation sit(situation);
|
|
constexpr double correction_time_ms = 1000;
|
|
|
|
// 1 -> 0 within correction_time_ms
|
|
const double errorOffsetFraction = (correction_time_ms - std::clamp(static_cast<double>(diffMs), 0.0, correction_time_ms)) / correction_time_ms;
|
|
Q_ASSERT_X(errorOffsetFraction >= 0 && errorOffsetFraction <= 1, Q_FUNC_INFO, "Offset fraction must be between 0 and 1");
|
|
|
|
{
|
|
// Heading
|
|
const double currentHeading = m_currentSitutation.getHeading().value(CAngleUnit::rad());
|
|
const double headingVelo = m_currentSitutation.getVelocity().getHeadingVelocity(CAngleUnit::rad(), CTimeUnit::s());
|
|
sit.setHeading(CHeading(currentHeading + diffMs/1000.0 * headingVelo + errorOffsetFraction * m_headingDelta, CAngleUnit::rad()));
|
|
}
|
|
|
|
{
|
|
// Pitch
|
|
const double currentPitch = m_currentSitutation.getPitch().value(CAngleUnit::rad());
|
|
const double pitchVelo = m_currentSitutation.getVelocity().getPitchVelocity(CAngleUnit::rad(), CTimeUnit::s());
|
|
sit.setPitch(CHeading(currentPitch + diffMs/1000.0 * pitchVelo + errorOffsetFraction * m_pitchDelta, CAngleUnit::rad()));
|
|
}
|
|
|
|
{
|
|
// Bank
|
|
const double currentBank = m_currentSitutation.getBank().value(CAngleUnit::rad());
|
|
const double bankVelo = m_currentSitutation.getVelocity().getRollVelocity(CAngleUnit::rad(), CTimeUnit::s());
|
|
sit.setBank(CHeading(currentBank + diffMs/1000.0 * bankVelo + errorOffsetFraction * m_bankDelta, CAngleUnit::rad()));
|
|
}
|
|
|
|
{
|
|
// Altitude
|
|
const double currentAltitude = m_currentSitutation.getCorrectedAltitude().value(CLengthUnit::ft());
|
|
const double altVelo = m_currentSitutation.getVelocity().getVelocityY(CSpeedUnit::m_s());
|
|
|
|
sit.setAltitude(CAltitude(currentAltitude + diffMs/1000.0 * altVelo + errorOffsetFraction * m_altitudeDelta, CLengthUnit::ft()));
|
|
}
|
|
|
|
{
|
|
// Position
|
|
const CCoordinateGeodetic pos = m_currentSitutation.getPosition();
|
|
|
|
// Latitude
|
|
const double latVelo = m_currentSitutation.getVelocity().getVelocityZ(CSpeedUnit::m_s());
|
|
CLength extrapolateLengthLat = CLength(latVelo * diffMs/1000.0, CLengthUnit::m());
|
|
CCoordinateGeodetic latShifted = pos;
|
|
// Extrapolate
|
|
double degLat = (extrapolateLengthLat.value(CLengthUnit::m()) >= 0 ? 0 : 180);
|
|
latShifted = pos.calculatePosition(extrapolateLengthLat.abs(), CAngle(degLat, CAngleUnit::deg()));
|
|
// Account delta since last update (error)
|
|
latShifted.setLatitude(latShifted.latitude() + CLatitude(errorOffsetFraction * m_latDelta, CAngleUnit::deg()));
|
|
|
|
// Longitude
|
|
const double lonVelo = m_currentSitutation.getVelocity().getVelocityX(CSpeedUnit::m_s());
|
|
CLength extrapolateLengthLon = CLength(lonVelo * diffMs/1000.0, CLengthUnit::m());
|
|
CCoordinateGeodetic lonShifted = pos;
|
|
// Extrapolate
|
|
double degLon = (extrapolateLengthLon.value(CLengthUnit::m()) >= 0 ? 90 : 270);
|
|
lonShifted = pos.calculatePosition(extrapolateLengthLon.abs(), CAngle(degLon, CAngleUnit::deg()));
|
|
// Account delta since last update (error)
|
|
lonShifted.setLongitude(lonShifted.longitude() + CLongitude(errorOffsetFraction * m_lonDelta, CAngleUnit::deg()));
|
|
|
|
CCoordinateGeodetic combined = sit.getPosition();
|
|
combined.setLatLong(latShifted.latitude(), lonShifted.longitude());
|
|
|
|
sit.setPosition(combined);
|
|
}
|
|
|
|
// Set ground factor directly from update (no extra/interpolation for now)
|
|
if (interpolateGndFactor)
|
|
{
|
|
sit.setOnGroundDetails(CAircraftSituation::OnGroundByInterpolation);
|
|
sit.setOnGround(m_currentSitutation.isOnGround());
|
|
sit.setOnGroundFactor(m_currentSitutation.isOnGround() ? 1.0 : 0.0);
|
|
}
|
|
|
|
sit.setMSecsSinceEpoch(getInterpolatedTime());
|
|
return sit;
|
|
}
|
|
|
|
CInterpolatorVelocity::CInterpolant CInterpolatorVelocity::getInterpolant(SituationLog &log)
|
|
{
|
|
if(!m_interpolant.isValid())
|
|
{
|
|
m_interpolant = CInterpolant{ *m_currentSituations.begin() };
|
|
m_lastSituation = *m_currentSituations.begin();
|
|
}
|
|
else if (m_lastSituation == *m_currentSituations.begin())
|
|
{
|
|
m_interpolant.update(m_currentTimeMsSinceEpoch);
|
|
}
|
|
else
|
|
{
|
|
m_interpolant = CInterpolant{ m_interpolant, *m_currentSituations.begin() };
|
|
m_lastSituation = *m_currentSituations.begin();
|
|
}
|
|
|
|
// Always changed (at least time)
|
|
m_interpolant.setRecalculated(true);
|
|
|
|
if (this->doLogging())
|
|
{
|
|
log.tsCurrent = m_currentTimeMsSinceEpoch;
|
|
// log.deltaSampleTimesMs = sampleDeltaTimeMs;
|
|
// log.simTimeFraction = simulationTimeFraction;
|
|
// log.deltaSampleTimesMs = sampleDeltaTimeMs;
|
|
// log.tsInterpolated = interpolatedTime;
|
|
log.interpolationSituations.clear();
|
|
log.interpolationSituations.push_back(*m_currentSituations.begin()); // current at front
|
|
log.interpolationSituations.push_back(m_interpolant.interpolatePositionAndAltitude(CAircraftSituation{}, true)); // interpolated second
|
|
log.interpolantRecalc = true;
|
|
}
|
|
|
|
return m_interpolant;
|
|
}
|
|
} // namespace
|