onur cayan
375 lines
14 KiB
C++
375 lines
14 KiB
C++
#include "RinexNavigation.h"
|
|
|
|
#include <QDir>
|
|
#include <QFile>
|
|
#include <QHash>
|
|
#include <QNetworkReply>
|
|
#include <QNetworkRequest>
|
|
#include <QStandardPaths>
|
|
#include <QUrl>
|
|
|
|
#include <zlib.h>
|
|
|
|
#include <algorithm>
|
|
#include <cmath>
|
|
#include <limits>
|
|
#include <utility>
|
|
|
|
namespace {
|
|
|
|
constexpr double GpsMu = 3.986005e14;
|
|
constexpr double EarthRotation = 7.2921151467e-5;
|
|
constexpr double GpsWeekSeconds = 604800.0;
|
|
constexpr double HalfGpsWeek = GpsWeekSeconds / 2.0;
|
|
constexpr double CurrentGpsUtcOffsetSeconds = 18.0;
|
|
constexpr int PositionUpdateIntervalMs = 60000;
|
|
|
|
double parseField(const QString &line, int field)
|
|
{
|
|
QString text = line.mid(4 + field * 19, 19).trimmed();
|
|
text.replace(QChar('D'), QChar('E'), Qt::CaseInsensitive);
|
|
bool ok = false;
|
|
const double value = text.toDouble(&ok);
|
|
return ok ? value : std::numeric_limits<double>::quiet_NaN();
|
|
}
|
|
|
|
bool finiteFields(std::initializer_list<double> values)
|
|
{
|
|
return std::all_of(values.begin(), values.end(), [](double value) {
|
|
return std::isfinite(value);
|
|
});
|
|
}
|
|
|
|
QByteArray gunzip(const QByteArray &input)
|
|
{
|
|
if (input.size() < 2 || static_cast<unsigned char>(input[0]) != 0x1f
|
|
|| static_cast<unsigned char>(input[1]) != 0x8b) {
|
|
return input;
|
|
}
|
|
|
|
z_stream stream{};
|
|
stream.next_in = reinterpret_cast<Bytef *>(const_cast<char *>(input.constData()));
|
|
stream.avail_in = static_cast<uInt>(input.size());
|
|
if (inflateInit2(&stream, 16 + MAX_WBITS) != Z_OK) {
|
|
return {};
|
|
}
|
|
|
|
QByteArray output;
|
|
char buffer[64 * 1024];
|
|
int result = Z_OK;
|
|
while (result == Z_OK) {
|
|
stream.next_out = reinterpret_cast<Bytef *>(buffer);
|
|
stream.avail_out = sizeof(buffer);
|
|
result = inflate(&stream, Z_NO_FLUSH);
|
|
output.append(buffer, static_cast<int>(sizeof(buffer) - stream.avail_out));
|
|
}
|
|
inflateEnd(&stream);
|
|
return result == Z_STREAM_END ? output : QByteArray{};
|
|
}
|
|
|
|
QDateTime gpsEpoch()
|
|
{
|
|
return QDateTime(QDate(1980, 1, 6), QTime(0, 0), Qt::UTC);
|
|
}
|
|
|
|
} // namespace
|
|
|
|
RinexNavigation::RinexNavigation(QObject *parent)
|
|
: QObject(parent)
|
|
{
|
|
m_positionTimer.setInterval(PositionUpdateIntervalMs);
|
|
connect(&m_positionTimer, &QTimer::timeout,
|
|
this, &RinexNavigation::updateCurrentPositions);
|
|
m_positionTimer.start();
|
|
}
|
|
|
|
void RinexNavigation::refresh()
|
|
{
|
|
if (m_loading) return;
|
|
if (m_ephemerides.isEmpty()) {
|
|
loadCache();
|
|
}
|
|
setLoading(true);
|
|
emit statusChanged(QStringLiteral("Guncel RINEX efemerisi indiriliyor..."), false);
|
|
requestDay(QDateTime::currentDateTimeUtc().date(), 0);
|
|
}
|
|
|
|
void RinexNavigation::requestDay(const QDate &date, int fallbackIndex)
|
|
{
|
|
const QString year = QString::number(date.year());
|
|
const QString day = QStringLiteral("%1").arg(date.dayOfYear(), 3, 10, QLatin1Char('0'));
|
|
// BKG's stream-derived file is updated during the UTC day; the final IGS
|
|
// combination is normally published later and is therefore unsuitable here.
|
|
const QString file = QStringLiteral("BRDC00WRD_S_") + year + day
|
|
+ QStringLiteral("0000_01D_MN.rnx.gz");
|
|
const QUrl url(QStringLiteral("https://igs.bkg.bund.de/root_ftp/IGS/BRDC/%1/%2/%3")
|
|
.arg(year, day, file));
|
|
QNetworkRequest request(url);
|
|
request.setHeader(QNetworkRequest::UserAgentHeader,
|
|
QStringLiteral("ANKASYS-LocationPlotter/1.0 (RINEX client)"));
|
|
request.setTransferTimeout(15000);
|
|
QNetworkReply *reply = m_network.get(request);
|
|
connect(reply, &QNetworkReply::finished, this, [this, reply, date, fallbackIndex] {
|
|
const bool ok = reply->error() == QNetworkReply::NoError;
|
|
const QByteArray data = ok ? reply->readAll() : QByteArray{};
|
|
reply->deleteLater();
|
|
if (!ok || data.isEmpty()) {
|
|
if (fallbackIndex < 2) {
|
|
requestDay(date.addDays(-1), fallbackIndex + 1);
|
|
} else {
|
|
setLoading(false);
|
|
if (!m_ephemerides.isEmpty()) {
|
|
publishCurrentMotion();
|
|
emit statusChanged(
|
|
QStringLiteral("RINEX indirilemedi; mevcut efemeris kullaniliyor"),
|
|
true);
|
|
} else if (!loadCache()) {
|
|
emit statusChanged(QStringLiteral("RINEX indirilemedi; internet baglantisini kontrol edin"), true);
|
|
}
|
|
}
|
|
return;
|
|
}
|
|
handleDownload(data, date, fallbackIndex);
|
|
});
|
|
}
|
|
|
|
void RinexNavigation::handleDownload(const QByteArray &compressed, const QDate &date,
|
|
int fallbackIndex)
|
|
{
|
|
const QByteArray rinex = gunzip(compressed);
|
|
QString error;
|
|
QVector<Ephemeris> ephemerides = parseEphemerides(rinex, &error);
|
|
const QDateTime now = QDateTime::currentDateTimeUtc();
|
|
const QVector<SatellitePosition> positions = propagateEphemerides(
|
|
selectEphemerides(ephemerides, now), now);
|
|
if (positions.isEmpty()) {
|
|
if (fallbackIndex < 2) {
|
|
requestDay(date.addDays(-1), fallbackIndex + 1);
|
|
return;
|
|
}
|
|
setLoading(false);
|
|
emit statusChanged(QStringLiteral("RINEX okunamadi: %1").arg(error), true);
|
|
return;
|
|
}
|
|
|
|
QFile cache(cachePath());
|
|
QDir().mkpath(QFileInfo(cache).absolutePath());
|
|
if (cache.open(QIODevice::WriteOnly)) cache.write(rinex);
|
|
m_ephemerides = std::move(ephemerides);
|
|
setLoading(false);
|
|
publishCurrentMotion();
|
|
emit statusChanged(QStringLiteral("%1 GPS uydusu · RINEX %2 · %3")
|
|
.arg(positions.size())
|
|
.arg(date.toString(QStringLiteral("dd.MM.yyyy")))
|
|
.arg(now.toLocalTime().toString(
|
|
QStringLiteral("HH:mm:ss t"))), false);
|
|
}
|
|
|
|
bool RinexNavigation::loadCache()
|
|
{
|
|
QFile cache(cachePath());
|
|
if (!cache.open(QIODevice::ReadOnly)) return false;
|
|
QString error;
|
|
QVector<Ephemeris> ephemerides = parseEphemerides(cache.readAll(), &error);
|
|
const QDateTime now = QDateTime::currentDateTimeUtc();
|
|
const QVector<SatellitePosition> positions = propagateEphemerides(
|
|
selectEphemerides(ephemerides, now), now);
|
|
if (positions.isEmpty()) return false;
|
|
m_ephemerides = std::move(ephemerides);
|
|
publishCurrentMotion();
|
|
emit statusChanged(QStringLiteral("%1 GPS uydusu · onbellekteki RINEX · %2")
|
|
.arg(positions.size())
|
|
.arg(now.toLocalTime().toString(
|
|
QStringLiteral("HH:mm:ss t"))), true);
|
|
return true;
|
|
}
|
|
|
|
QString RinexNavigation::cachePath() const
|
|
{
|
|
return QStandardPaths::writableLocation(QStandardPaths::CacheLocation)
|
|
+ QStringLiteral("/latest-navigation.rnx");
|
|
}
|
|
|
|
void RinexNavigation::updateCurrentPositions()
|
|
{
|
|
if (m_ephemerides.isEmpty()) return;
|
|
publishCurrentMotion();
|
|
}
|
|
|
|
void RinexNavigation::publishCurrentMotion()
|
|
{
|
|
if (m_ephemerides.isEmpty()) return;
|
|
|
|
const QDateTime now = QDateTime::currentDateTimeUtc();
|
|
const QVector<Ephemeris> selected = selectEphemerides(m_ephemerides, now);
|
|
const QVector<SatellitePosition> current = propagateEphemerides(selected, now);
|
|
if (current.isEmpty()) return;
|
|
|
|
const QVector<SatellitePosition> next = propagateEphemerides(
|
|
selected, now.addMSecs(PositionUpdateIntervalMs));
|
|
emit satellitesUpdated(current);
|
|
if (!next.isEmpty()) {
|
|
emit satelliteMotionUpdated(current, next, PositionUpdateIntervalMs);
|
|
}
|
|
}
|
|
|
|
void RinexNavigation::setLoading(bool loading)
|
|
{
|
|
if (m_loading == loading) return;
|
|
m_loading = loading;
|
|
emit loadingChanged(loading);
|
|
}
|
|
|
|
QVector<RinexNavigation::Ephemeris> RinexNavigation::parseEphemerides(
|
|
const QByteArray &rinex, QString *error)
|
|
{
|
|
const QString text = QString::fromLatin1(rinex);
|
|
const QStringList lines = text.split('\n');
|
|
int index = 0;
|
|
while (index < lines.size() && !lines[index].contains(QStringLiteral("END OF HEADER"))) ++index;
|
|
if (index >= lines.size()) {
|
|
if (error) *error = QStringLiteral("END OF HEADER bulunamadi");
|
|
return {};
|
|
}
|
|
++index;
|
|
|
|
QVector<Ephemeris> result;
|
|
result.reserve(512);
|
|
while (index < lines.size()) {
|
|
const QString first = lines[index];
|
|
if (first.size() < 3) { ++index; continue; }
|
|
const QChar system = first[0];
|
|
if (!system.isLetter()) { ++index; continue; }
|
|
const int blockLines = (system == QLatin1Char('R') || system == QLatin1Char('S')) ? 4 : 8;
|
|
if (index + blockLines > lines.size()) break;
|
|
if (system != QLatin1Char('G')) { index += blockLines; continue; }
|
|
|
|
const QStringList epoch = first.mid(3, 20).simplified().split(' ');
|
|
if (epoch.size() < 6) { index += 8; continue; }
|
|
bool dateOk = false;
|
|
const int year = epoch[0].toInt(&dateOk);
|
|
const QDateTime toc(QDate(year, epoch[1].toInt(), epoch[2].toInt()),
|
|
QTime(epoch[3].toInt(), epoch[4].toInt(),
|
|
static_cast<int>(epoch[5].toDouble())), Qt::UTC);
|
|
if (!dateOk || !toc.isValid()) { index += 8; continue; }
|
|
|
|
Ephemeris eph;
|
|
eph.prn = first.left(3).trimmed().toUpper();
|
|
eph.toc = toc;
|
|
eph.crs = parseField(lines[index + 1], 1);
|
|
eph.deltaN = parseField(lines[index + 1], 2);
|
|
eph.m0 = parseField(lines[index + 1], 3);
|
|
eph.cuc = parseField(lines[index + 2], 0);
|
|
eph.eccentricity = parseField(lines[index + 2], 1);
|
|
eph.cus = parseField(lines[index + 2], 2);
|
|
eph.sqrtA = parseField(lines[index + 2], 3);
|
|
eph.toe = parseField(lines[index + 3], 0);
|
|
eph.cic = parseField(lines[index + 3], 1);
|
|
eph.omega0 = parseField(lines[index + 3], 2);
|
|
eph.cis = parseField(lines[index + 3], 3);
|
|
eph.i0 = parseField(lines[index + 4], 0);
|
|
eph.crc = parseField(lines[index + 4], 1);
|
|
eph.omega = parseField(lines[index + 4], 2);
|
|
eph.omegaDot = parseField(lines[index + 4], 3);
|
|
eph.idot = parseField(lines[index + 5], 0);
|
|
eph.gpsWeek = static_cast<int>(std::llround(parseField(lines[index + 5], 2)));
|
|
const double health = parseField(lines[index + 6], 1);
|
|
eph.healthy = !std::isfinite(health) || std::llround(health) == 0;
|
|
|
|
if (finiteFields({eph.crs, eph.deltaN, eph.m0, eph.cuc, eph.eccentricity,
|
|
eph.cus, eph.sqrtA, eph.toe, eph.cic, eph.omega0, eph.cis,
|
|
eph.i0, eph.crc, eph.omega, eph.omegaDot, eph.idot})
|
|
&& eph.sqrtA > 4000.0 && eph.gpsWeek > 0) {
|
|
result.push_back(eph);
|
|
}
|
|
index += 8;
|
|
}
|
|
if (result.isEmpty() && error) *error = QStringLiteral("GPS efemerisi bulunamadi");
|
|
return result;
|
|
}
|
|
|
|
QVector<RinexNavigation::Ephemeris> RinexNavigation::selectEphemerides(
|
|
const QVector<Ephemeris> &ephemerides, const QDateTime &whenUtc)
|
|
{
|
|
QHash<QString, Ephemeris> closest;
|
|
QHash<QString, qint64> closestAge;
|
|
// GPS navigation epochs are labelled in GPS system time, not UTC.
|
|
const QDateTime whenGps = whenUtc.addSecs(
|
|
static_cast<qint64>(CurrentGpsUtcOffsetSeconds));
|
|
for (const Ephemeris &eph : ephemerides) {
|
|
const qint64 age = std::abs(eph.toc.msecsTo(whenGps));
|
|
if (!closestAge.contains(eph.prn) || age < closestAge[eph.prn]) {
|
|
closest[eph.prn] = eph;
|
|
closestAge[eph.prn] = age;
|
|
}
|
|
}
|
|
|
|
QVector<Ephemeris> result;
|
|
result.reserve(closest.size());
|
|
for (const Ephemeris &eph : closest) result.push_back(eph);
|
|
std::sort(result.begin(), result.end(), [](const auto &a, const auto &b) {
|
|
return a.prn < b.prn;
|
|
});
|
|
return result;
|
|
}
|
|
|
|
QVector<SatellitePosition> RinexNavigation::propagateEphemerides(
|
|
const QVector<Ephemeris> &ephemerides, const QDateTime &whenUtc)
|
|
{
|
|
QVector<SatellitePosition> result;
|
|
result.reserve(ephemerides.size());
|
|
for (const Ephemeris &eph : ephemerides) result.push_back(propagate(eph, whenUtc));
|
|
std::sort(result.begin(), result.end(), [](const auto &a, const auto &b) { return a.prn < b.prn; });
|
|
return result;
|
|
}
|
|
|
|
SatellitePosition RinexNavigation::propagate(const Ephemeris &eph, const QDateTime &whenUtc)
|
|
{
|
|
const double gpsSeconds = gpsEpoch().msecsTo(whenUtc) / 1000.0
|
|
+ CurrentGpsUtcOffsetSeconds;
|
|
const double absoluteToe = eph.gpsWeek * GpsWeekSeconds + eph.toe;
|
|
double tk = gpsSeconds - absoluteToe;
|
|
while (tk > HalfGpsWeek) tk -= GpsWeekSeconds;
|
|
while (tk < -HalfGpsWeek) tk += GpsWeekSeconds;
|
|
|
|
const double a = eph.sqrtA * eph.sqrtA;
|
|
const double meanMotion = std::sqrt(GpsMu / (a * a * a)) + eph.deltaN;
|
|
const double meanAnomaly = eph.m0 + meanMotion * tk;
|
|
double eccentricAnomaly = meanAnomaly;
|
|
for (int iteration = 0; iteration < 12; ++iteration) {
|
|
eccentricAnomaly = meanAnomaly + eph.eccentricity * std::sin(eccentricAnomaly);
|
|
}
|
|
|
|
const double trueAnomaly = std::atan2(
|
|
std::sqrt(1.0 - eph.eccentricity * eph.eccentricity) * std::sin(eccentricAnomaly),
|
|
std::cos(eccentricAnomaly) - eph.eccentricity);
|
|
const double phi = trueAnomaly + eph.omega;
|
|
const double twoPhi = 2.0 * phi;
|
|
const double u = phi + eph.cus * std::sin(twoPhi) + eph.cuc * std::cos(twoPhi);
|
|
const double radius = a * (1.0 - eph.eccentricity * std::cos(eccentricAnomaly))
|
|
+ eph.crs * std::sin(twoPhi) + eph.crc * std::cos(twoPhi);
|
|
const double inclination = eph.i0 + eph.idot * tk
|
|
+ eph.cis * std::sin(twoPhi) + eph.cic * std::cos(twoPhi);
|
|
const double orbitalX = radius * std::cos(u);
|
|
const double orbitalY = radius * std::sin(u);
|
|
const double ascendingNode = eph.omega0 + (eph.omegaDot - EarthRotation) * tk
|
|
- EarthRotation * eph.toe;
|
|
|
|
SatellitePosition result;
|
|
result.prn = eph.prn;
|
|
result.x = orbitalX * std::cos(ascendingNode)
|
|
- orbitalY * std::cos(inclination) * std::sin(ascendingNode);
|
|
result.y = orbitalX * std::sin(ascendingNode)
|
|
+ orbitalY * std::cos(inclination) * std::cos(ascendingNode);
|
|
result.z = orbitalY * std::sin(inclination);
|
|
result.healthy = eph.healthy;
|
|
return result;
|
|
}
|
|
|
|
QVector<SatellitePosition> RinexNavigation::parseGpsNavigation(
|
|
const QByteArray &rinex, const QDateTime &whenUtc, QString *error)
|
|
{
|
|
const QVector<Ephemeris> ephemerides = parseEphemerides(rinex, error);
|
|
return propagateEphemerides(selectEphemerides(ephemerides, whenUtc), whenUtc);
|
|
}
|