onur/location_plotter
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
8b7256ed9d4a37ba35bc394c672e4eb6cc4d2dd9
location_plotter/tests/parser_tests.cpp
T

280 lines
11 KiB
C++
Raw Blame History

#include "../src/LocationParser.h"
#include "../src/RinexNavigation.h"
#include <QCoreApplication>
#include <QFile>
#include <cassert>
#include <cmath>
namespace {
bool closeTo(double left, double right, double tolerance = 0.000001)
{
return std::fabs(left - right) < tolerance;
}
QString rinexField(double value)
{
return QStringLiteral("%1").arg(value, 19, 'E', 12);
}
QString rinexContinuation(std::initializer_list<double> values)
{
QString line(4, QLatin1Char(' '));
for (double value : values) line += rinexField(value);
return line + QLatin1Char('\n');
}
QByteArray syntheticGpsNavigation(const QDateTime &gpsEpochLabel, int gpsWeek, double toe)
{
QString rinex = QStringLiteral(
" 3.05 N: GNSS NAV DATA M: MIXED RINEX VERSION / TYPE\n"
" END OF HEADER\n");
rinex += QStringLiteral("G01 %1").arg(
gpsEpochLabel.toString(QStringLiteral("yyyy MM dd HH mm ss")));
rinex += rinexField(0.0) + rinexField(0.0) + rinexField(0.0) + QLatin1Char('\n');
rinex += rinexContinuation({1.0, 0.0, 0.0, 0.0});
rinex += rinexContinuation({0.0, 0.0, 0.0, 5153.7954775});
rinex += rinexContinuation({toe, 0.0, 0.0, 0.0});
rinex += rinexContinuation({0.0, 0.0, 0.0, 0.0});
rinex += rinexContinuation({0.0, 0.0, static_cast<double>(gpsWeek), 0.0});
rinex += rinexContinuation({2.0, 0.0, 0.0, 1.0});
rinex += rinexContinuation({0.0, 0.0, 0.0, 0.0});
return rinex.toLatin1();
}
} // namespace
int main(int argc, char **argv)
{
QCoreApplication app(argc, argv);
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("DBG lat=41.0082000 long=28.9784000 hheight=123.45 sats=7"));
assert(parsed.has_value());
assert(parsed->hasLocation());
assert(closeTo(*parsed->latitude, 41.0082));
assert(closeTo(*parsed->longitude, 28.9784));
assert(closeTo(*parsed->hheight, 123.45));
assert(parsed->satellites == 7);
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("latitude: -12,5 longitude: 33,25 height: 99 activeSats: 4"));
assert(parsed.has_value());
assert(parsed->hasLocation());
assert(closeTo(*parsed->latitude, -12.5));
assert(closeTo(*parsed->longitude, 33.25));
assert(parsed->latitudeText == QStringLiteral("-12.5"));
assert(parsed->longitudeText == QStringLiteral("33.25"));
assert(closeTo(*parsed->hheight, 99.0));
assert(parsed->satellites == 4);
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral("satellites_active=11"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(parsed->satellites == 11);
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("lat=200 long=28 hheight=100 active_sats=8"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(parsed->latitudeText.isEmpty());
assert(parsed->longitudeText.isEmpty());
assert(!parsed->hheight.has_value());
assert(parsed->satellites == 8);
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("lattitude : 40.78925034214206846173"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(closeTo(*parsed->latitude, 40.78925034214206846173));
assert(parsed->latitudeText == QStringLiteral("40.78925034214206846173"));
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("longitude : 29.46277530841727809729"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(closeTo(*parsed->longitude, 29.46277530841727809729));
assert(parsed->longitudeText == QStringLiteral("29.46277530841727809729"));
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("height : -89.05779508221894502640 "));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(closeTo(*parsed->hheight, -89.05779508221894502640));
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("NAV solve start sats:4 mode:periodic abs_span_ms:2.176"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(parsed->satellites == 4);
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("[NAV] navigation software running with 5 satellites"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(parsed->satellites == 5);
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral("GDOP : 3.9576744550"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(closeTo(*parsed->dop.gdop, 3.9576744550));
}
{
const auto parsed = LocationParser::parseLine(
QStringLiteral("PDOP : 3.2540598660 HDOP : 2.3631161789 VDOP : 2.2370935468 TDOP : 2.2526165852"));
assert(parsed.has_value());
assert(!parsed->hasLocation());
assert(closeTo(*parsed->dop.pdop, 3.2540598660));
assert(closeTo(*parsed->dop.hdop, 2.3631161789));
assert(closeTo(*parsed->dop.vdop, 2.2370935468));
assert(closeTo(*parsed->dop.tdop, 2.2526165852));
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral("plain debug noise"));
assert(!parsed.has_value());
}
{
const QSet<QString> prns = LocationParser::parseGpsPrns(
QStringLiteral("tracking PRNs: 3, G08, 14; SV=G21 SVID: 27"));
assert(prns.contains(QStringLiteral("G03")));
assert(prns.contains(QStringLiteral("G08")));
assert(prns.contains(QStringLiteral("G14")));
assert(prns.contains(QStringLiteral("G21")));
assert(prns.contains(QStringLiteral("G27")));
assert(prns.size() == 5);
}
{
const QString line = QStringLiteral(
"[OK] [TRK] tracked satellites=5 svn=[2,3,5,8,30]");
const auto parsed = LocationParser::parseLine(line);
assert(parsed.has_value());
assert(parsed->satellites == 5);
const QSet<QString> prns = LocationParser::parseGpsPrns(line);
assert(prns == QSet<QString>({QStringLiteral("G03"), QStringLiteral("G04"),
QStringLiteral("G06"), QStringLiteral("G09"),
QStringLiteral("G31")}));
}
{
const QSet<QString> prns = LocationParser::parseGpsPrns(
QStringLiteral("[TRK] tracked satellites=2 svn=[0,31]"));
assert(prns == QSet<QString>({QStringLiteral("G01"), QStringLiteral("G32")}));
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral(
"[OK] [NAV] DOP GDOP=7.0907891622 PDOP=5.6333862175 "
"HDOP=2.9819528958 VDOP=4.7794348204 TDOP=4.3063035967"));
assert(parsed.has_value());
assert(closeTo(*parsed->dop.gdop, 7.0907891622));
assert(closeTo(*parsed->dop.pdop, 5.6333862175));
assert(closeTo(*parsed->dop.hdop, 2.9819528958));
assert(closeTo(*parsed->dop.vdop, 4.7794348204));
assert(closeTo(*parsed->dop.tdop, 4.3063035967));
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral(
"[OK] [NAV] position lat=40.77831900791127850425 "
"lon=29.43579105837637044374 height=198.80186179187148809433"));
assert(parsed.has_value());
assert(parsed->hasLocation());
assert(closeTo(*parsed->latitude, 40.77831900791127850425));
assert(closeTo(*parsed->longitude, 29.43579105837637044374));
assert(closeTo(*parsed->hheight, 198.80186179187148809433));
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral(
"[LOCATION] source=imu_corrected mode=dead_reckoning "
"lat=40.7356 lon=31.5985 height=812.7 gps_age_ms=3180 "
"gdop=4.7 pdop=4.1 hdop=2.7 vdop=3.0 tdop=2.3 tracked=6 used=5"));
assert(parsed.has_value());
assert(parsed->hasLocation());
assert(parsed->source == LocationSource::ImuCorrected);
assert(parsed->fusionMode == FusionMode::DeadReckoning);
assert(parsed->gpsAgeMs == 3180);
assert(parsed->satellites == 6);
assert(closeTo(*parsed->dop.hdop, 2.7));
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral(
"[LOCATION] source=gps lat=40.7357 lon=31.5986 height=811 tracked=5 used=5"));
assert(parsed.has_value());
assert(parsed->source == LocationSource::Gps);
assert(parsed->satellites == 5);
}
{
const auto parsed = LocationParser::parseLine(QStringLiteral(
"[LOCATION] source=trajectory lat=40.7358 lon=31.5987 height=810 tracked=4 used=4"));
assert(parsed.has_value());
assert(parsed->hasLocation());
assert(parsed->source == LocationSource::Trajectory);
assert(parsed->satellites == 4);
}
{
constexpr int gpsWeek = 2428;
constexpr double toe = 403200.0;
const QDateTime gpsEpoch(QDate(1980, 1, 6), QTime(0, 0), Qt::UTC);
const QDateTime gpsLabel = gpsEpoch.addSecs(
static_cast<qint64>(gpsWeek * 604800.0 + toe));
const QDateTime matchingUtc = gpsLabel.addSecs(-18);
QString error;
const auto satellites = RinexNavigation::parseGpsNavigation(
syntheticGpsNavigation(gpsLabel, gpsWeek, toe), matchingUtc, &error);
assert(error.isEmpty());
assert(satellites.size() == 1);
const double semiMajorAxis = 5153.7954775 * 5153.7954775;
const double node = -7.2921151467e-5 * toe;
assert(closeTo(satellites[0].x, semiMajorAxis * std::cos(node), 0.1));
assert(closeTo(satellites[0].y, semiMajorAxis * std::sin(node), 0.1));
assert(closeTo(satellites[0].z, 0.0, 0.1));
}
const QString rinexPath = qEnvironmentVariable("LOCATION_PLOTTER_TEST_RINEX");
if (!rinexPath.isEmpty()) {
QFile file(rinexPath);
assert(file.open(QIODevice::ReadOnly));
QString error;
const auto satellites = RinexNavigation::parseGpsNavigation(
file.readAll(), QDateTime::currentDateTimeUtc(), &error);
assert(satellites.size() >= 20);
for (const auto &satellite : satellites) {
assert(satellite.prn.startsWith(QStringLiteral("G")));
assert(std::isfinite(satellite.x));
assert(std::isfinite(satellite.y));
assert(std::isfinite(satellite.z));
}
}
return 0;
}
Reference in New Issue View Git Blame Copy Permalink