267 lines
9.9 KiB
C++
267 lines
9.9 KiB
C++
#include "LocationParser.h"
|
|
|
|
#include <QRegularExpression>
|
|
|
|
#include <cmath>
|
|
|
|
namespace {
|
|
|
|
double toDouble(QString value, bool *ok)
|
|
{
|
|
value.replace(',', '.');
|
|
return value.toDouble(ok);
|
|
}
|
|
|
|
QString normalizedCoordinateText(QString value)
|
|
{
|
|
value.replace(',', '.');
|
|
if (value.startsWith('+')) {
|
|
value.remove(0, 1);
|
|
}
|
|
return value;
|
|
}
|
|
|
|
QString normalizeKey(QString key)
|
|
{
|
|
key = key.toLower();
|
|
key.remove('_');
|
|
key.remove('-');
|
|
key.remove(' ');
|
|
|
|
if (key == "lat" || key == "latitude" || key == "lattitude") {
|
|
return "latitude";
|
|
}
|
|
|
|
if (key == "long" || key == "longitude" || key == "lon" || key == "lng") {
|
|
return "longitude";
|
|
}
|
|
|
|
if (key == "hheight" || key == "height" || key == "alt" || key == "altitude") {
|
|
return "hheight";
|
|
}
|
|
|
|
if (key == "sat" || key == "sats" || key == "satellite" || key == "satellites"
|
|
|| key == "activesat" || key == "activesats" || key == "activesatellite"
|
|
|| key == "activesatellites" || key == "numsat" || key == "numsats"
|
|
|| key == "numbersat" || key == "numbersats" || key == "satactive"
|
|
|| key == "satsactive" || key == "satelliteactive" || key == "satellitesactive"
|
|
|| key == "uydu" || key == "uydular") {
|
|
return "satellites";
|
|
}
|
|
|
|
if (key == "tracked") {
|
|
return "satellites";
|
|
}
|
|
|
|
if (key == "gdop" || key == "pdop" || key == "hdop" || key == "vdop" || key == "tdop") {
|
|
return key;
|
|
}
|
|
|
|
return {};
|
|
}
|
|
|
|
bool isValidLatitude(double latitude)
|
|
{
|
|
return std::isfinite(latitude) && latitude >= -90.0 && latitude <= 90.0;
|
|
}
|
|
|
|
bool isValidLongitude(double longitude)
|
|
{
|
|
return std::isfinite(longitude) && longitude >= -180.0 && longitude <= 180.0;
|
|
}
|
|
|
|
} // namespace
|
|
|
|
std::optional<ParsedTelemetry> LocationParser::parseLine(const QString &line)
|
|
{
|
|
const QString trimmed = line.trimmed();
|
|
if (trimmed.isEmpty()) {
|
|
return std::nullopt;
|
|
}
|
|
|
|
static const QRegularExpression labeledValueRegex(
|
|
QStringLiteral(R"((^|[^A-Za-z0-9_])(?<key>lat(?:t?itude)?|long(?:itude)?|lon|lng|hheight|height|alt(?:itude)?|active[_ -]?sat(?:ellite)?s?|num(?:ber)?[_ -]?sat(?:ellite)?s?|sat(?:ellite)?s?[_ -]?active|sat(?:ellite)?s?|sats|tracked|uydu(?:lar)?|[gphvt][_ -]?dop)(?:\s*(?:=|:|->)?\s*)(?<value>[+-]?(?:\d+(?:[\.,]\d+)?|[\.,]\d+)(?:[eE][+-]?\d+)?))"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
static const QRegularExpression satelliteCountPhraseRegex(
|
|
QStringLiteral(R"((^|[^A-Za-z0-9_\.])(?<value>\d+)\s+(?:active\s+)?(?:sat(?:ellite)?s?|uydu(?:lar)?)(?=$|[\s,.;)\]]))"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
|
|
ParsedTelemetry telemetry;
|
|
telemetry.rawLine = trimmed;
|
|
static const QRegularExpression sourceRegex(
|
|
QStringLiteral(R"((?:^|\s)source\s*=\s*(?<source>gps|imu[_-]?corrected|trajectory|ground[_-]?truth)(?=$|\s))"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
const QRegularExpressionMatch sourceMatch = sourceRegex.match(trimmed);
|
|
if (sourceMatch.hasMatch()) {
|
|
const QString source = sourceMatch.captured(QStringLiteral("source"));
|
|
if (source.startsWith(QStringLiteral("imu"), Qt::CaseInsensitive)) {
|
|
telemetry.source = LocationSource::ImuCorrected;
|
|
} else if (source.startsWith(QStringLiteral("trajectory"), Qt::CaseInsensitive)
|
|
|| source.startsWith(QStringLiteral("ground"), Qt::CaseInsensitive)) {
|
|
telemetry.source = LocationSource::Trajectory;
|
|
} else {
|
|
telemetry.source = LocationSource::Gps;
|
|
}
|
|
}
|
|
static const QRegularExpression fusionModeRegex(
|
|
QStringLiteral(R"((?:^|\s)mode\s*=\s*(?<mode>gps[_-]?aided|dead[_-]?reckoning)(?=$|\s))"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
const QRegularExpressionMatch fusionModeMatch = fusionModeRegex.match(trimmed);
|
|
if (fusionModeMatch.hasMatch()) {
|
|
telemetry.fusionMode = fusionModeMatch.captured(QStringLiteral("mode")).startsWith(
|
|
QStringLiteral("dead"), Qt::CaseInsensitive)
|
|
? FusionMode::DeadReckoning : FusionMode::GpsAided;
|
|
}
|
|
static const QRegularExpression gpsAgeRegex(
|
|
QStringLiteral(R"((?:^|\s)gps[_-]?age[_-]?ms\s*=\s*(?<age>\d+)(?=$|\s))"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
const QRegularExpressionMatch gpsAgeMatch = gpsAgeRegex.match(trimmed);
|
|
if (gpsAgeMatch.hasMatch()) {
|
|
bool ok = false;
|
|
const qint64 ageMs = gpsAgeMatch.captured(QStringLiteral("age")).toLongLong(&ok);
|
|
if (ok) {
|
|
telemetry.gpsAgeMs = ageMs;
|
|
}
|
|
}
|
|
bool hadCoordinateField = false;
|
|
bool hadLatitudeField = false;
|
|
bool hadLongitudeField = false;
|
|
|
|
auto matches = labeledValueRegex.globalMatch(trimmed);
|
|
while (matches.hasNext()) {
|
|
const QRegularExpressionMatch match = matches.next();
|
|
const QString field = normalizeKey(match.captured(QStringLiteral("key")));
|
|
if (field.isEmpty()) {
|
|
continue;
|
|
}
|
|
|
|
bool ok = false;
|
|
const double value = toDouble(match.captured(QStringLiteral("value")), &ok);
|
|
if (!ok || !std::isfinite(value)) {
|
|
continue;
|
|
}
|
|
|
|
if (field == "latitude") {
|
|
hadCoordinateField = true;
|
|
hadLatitudeField = true;
|
|
telemetry.latitude = value;
|
|
telemetry.latitudeText = normalizedCoordinateText(
|
|
match.captured(QStringLiteral("value")));
|
|
} else if (field == "longitude") {
|
|
hadCoordinateField = true;
|
|
hadLongitudeField = true;
|
|
telemetry.longitude = value;
|
|
telemetry.longitudeText = normalizedCoordinateText(
|
|
match.captured(QStringLiteral("value")));
|
|
} else if (field == "hheight") {
|
|
telemetry.hheight = value;
|
|
} else if (field == "satellites") {
|
|
const int count = static_cast<int>(std::llround(value));
|
|
if (count >= 0) {
|
|
telemetry.satellites = count;
|
|
}
|
|
} else if (field == "gdop") {
|
|
if (value >= 0.0) {
|
|
telemetry.dop.gdop = value;
|
|
}
|
|
} else if (field == "pdop") {
|
|
if (value >= 0.0) {
|
|
telemetry.dop.pdop = value;
|
|
}
|
|
} else if (field == "hdop") {
|
|
if (value >= 0.0) {
|
|
telemetry.dop.hdop = value;
|
|
}
|
|
} else if (field == "vdop") {
|
|
if (value >= 0.0) {
|
|
telemetry.dop.vdop = value;
|
|
}
|
|
} else if (field == "tdop") {
|
|
if (value >= 0.0) {
|
|
telemetry.dop.tdop = value;
|
|
}
|
|
}
|
|
}
|
|
|
|
auto satellitePhraseMatches = satelliteCountPhraseRegex.globalMatch(trimmed);
|
|
while (satellitePhraseMatches.hasNext()) {
|
|
const QRegularExpressionMatch match = satellitePhraseMatches.next();
|
|
bool ok = false;
|
|
const int count = match.captured(QStringLiteral("value")).toInt(&ok);
|
|
if (ok && count >= 0) {
|
|
telemetry.satellites = count;
|
|
}
|
|
}
|
|
|
|
if (telemetry.latitude.has_value() && !isValidLatitude(*telemetry.latitude)) {
|
|
telemetry.latitude.reset();
|
|
telemetry.latitudeText.clear();
|
|
}
|
|
if (telemetry.longitude.has_value() && !isValidLongitude(*telemetry.longitude)) {
|
|
telemetry.longitude.reset();
|
|
telemetry.longitudeText.clear();
|
|
}
|
|
|
|
if (hadLatitudeField && hadLongitudeField
|
|
&& (!telemetry.hasLocation()
|
|
|| !isValidCoordinate(*telemetry.latitude, *telemetry.longitude))) {
|
|
telemetry.latitude.reset();
|
|
telemetry.longitude.reset();
|
|
telemetry.latitudeText.clear();
|
|
telemetry.longitudeText.clear();
|
|
telemetry.hheight.reset();
|
|
} else if (hadCoordinateField && !telemetry.latitude.has_value()
|
|
&& !telemetry.longitude.has_value()) {
|
|
telemetry.hheight.reset();
|
|
}
|
|
|
|
if (!telemetry.hasAnyValue()) {
|
|
return std::nullopt;
|
|
}
|
|
|
|
return telemetry;
|
|
}
|
|
|
|
bool LocationParser::isValidCoordinate(double latitude, double longitude)
|
|
{
|
|
return std::isfinite(latitude) && std::isfinite(longitude)
|
|
&& latitude >= -90.0 && latitude <= 90.0
|
|
&& longitude >= -180.0 && longitude <= 180.0;
|
|
}
|
|
|
|
QSet<QString> LocationParser::parseGpsPrns(const QString &line)
|
|
{
|
|
QSet<QString> result;
|
|
const auto addPrn = [&result](QString value, bool zeroBased = false) {
|
|
value = value.trimmed().toUpper();
|
|
if (value.startsWith(QLatin1Char('G'))) value.remove(0, 1);
|
|
bool ok = false;
|
|
int number = value.toInt(&ok);
|
|
if (ok && zeroBased) ++number;
|
|
if (ok && number >= 1 && number <= 63) {
|
|
result.insert(QStringLiteral("G%1").arg(number, 2, 10, QLatin1Char('0')));
|
|
}
|
|
};
|
|
|
|
static const QRegularExpression gpsToken(
|
|
QStringLiteral(R"((?:^|[^A-Za-z0-9])G(?<prn>\d{1,2})(?=$|[^0-9]))"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
auto gpsMatches = gpsToken.globalMatch(line);
|
|
while (gpsMatches.hasNext()) addPrn(gpsMatches.next().captured(QStringLiteral("prn")));
|
|
|
|
static const QRegularExpression labeledList(
|
|
QStringLiteral(R"((?<label>\bPRNS?\b|\bSVS?\b|\bSVNS?\b|\bSVIDS?\b)\s*(?:=|:|->)?\s*[\[(]?\s*(?<list>(?:G?\d{1,2})(?:\s*[,;/| ]\s*G?\d{1,2})*)\s*[\])]?)"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
static const QRegularExpression number(QStringLiteral(R"(G?(\d{1,2}))"),
|
|
QRegularExpression::CaseInsensitiveOption);
|
|
auto listMatches = labeledList.globalMatch(line);
|
|
while (listMatches.hasNext()) {
|
|
const QRegularExpressionMatch listMatch = listMatches.next();
|
|
const bool zeroBased = listMatch.captured(QStringLiteral("label")).startsWith(
|
|
QStringLiteral("SVN"), Qt::CaseInsensitive);
|
|
auto values = number.globalMatch(listMatch.captured(QStringLiteral("list")));
|
|
while (values.hasNext()) addPrn(values.next().captured(1), zeroBased);
|
|
}
|
|
return result;
|
|
}
|