From 2bdab0f4494bf15b82e38100eb2fb93a4fd025ec Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Martin=20T=C5=AFma?= <tumic@cbox.cz>
Date: Tue, 22 Nov 2016 00:13:41 +0100
Subject: [PATCH] NMEA parser improvements

---
 src/nmeaparser.cpp | 471 ++++++++++++++++++++++++++-------------------
 src/nmeaparser.h   |  10 +
 2 files changed, 284 insertions(+), 197 deletions(-)

diff --git a/src/nmeaparser.cpp b/src/nmeaparser.cpp
index 61d42a22..cef67d32 100644
--- a/src/nmeaparser.cpp
+++ b/src/nmeaparser.cpp
@@ -3,104 +3,6 @@
 #include "nmeaparser.h"
 
 
-static bool readTime(const char *data, int len, QTime &time)
-{
-	int h, m, s, ms = 0;
-
-	if (len < 6)
-		return false;
-
-	h = str2int(data, 2);
-	m = str2int(data + 2, 2);
-	s = str2int(data + 4, 2);
-	if (h < 0 || m < 0 || s < 0)
-		return false;
-
-	if (len > 7 && data[6] == '.') {
-		if ((ms = str2int(data + 7, len - 7)) < 0)
-			return false;
-	}
-
-	time = QTime(h, m, s, ms);
-	if (!time.isValid())
-		return false;
-
-	return true;
-}
-
-static bool readDate(const char *data, int len, QDate &date)
-{
-	int y, m, d;
-
-	if (len != 6)
-		return false;
-
-	d = str2int(data, 2);
-	m = str2int(data + 2, 2);
-	y = str2int(data + 4, 2);
-	if (d < 0 || m < 0 || y < 0)
-		return false;
-
-	date = QDate(2000 + y, m, d);
-	if (!date.isValid())
-		return false;
-
-	return true;
-}
-
-static bool readLat(const char *data, int len, qreal &lat)
-{
-	int d, mi;
-	qreal mf;
-	bool ok;
-
-	if (len < 7 || data[4] != '.')
-		return false;
-
-	d = str2int(data, 2);
-	mi = str2int(data + 2, 2);
-	mf = QString(QByteArray::fromRawData(data + 4, len - 4)).toFloat(&ok);
-	if (d < 0 || mi < 0 || !ok)
-		return false;
-
-	lat = d + (((qreal)mi + mf) / 60.0);
-	if (lat > 90)
-		return false;
-
-	return true;
-}
-
-static bool readLon(const char *data, int len, qreal &lon)
-{
-	int d, mi;
-	qreal mf;
-	bool ok;
-
-	if (len < 8 || data[5] != '.')
-		return false;
-
-	d = str2int(data, 3);
-	mi = str2int(data + 3, 2);
-	mf = QString(QByteArray::fromRawData(data + 5, len - 5)).toFloat(&ok);
-	if (d < 0 || mi < 0 || !ok)
-		return false;
-
-	lon = d + (((qreal)mi + mf) / 60.0);
-	if (lon > 180)
-		return false;
-
-	return true;
-}
-
-static bool readFloat(const char *data, int len, qreal &f)
-{
-	bool ok;
-
-	f = QString(QByteArray::fromRawData(data, len)).toFloat(&ok);
-
-	return ok;
-}
-
 static bool validSentence(const char  *line, int len)
 {
 	const char *lp;
@@ -117,60 +19,253 @@ static bool validSentence(const char  *line, int len)
 	return true;
 }
 
+static bool readFloat(const char *data, int len, qreal &f)
+{
+	bool ok;
+
+	f = QString(QByteArray::fromRawData(data, len)).toFloat(&ok);
+
+	return ok;
+}
+
+bool NMEAParser::readAltitude(const char *data, int len, qreal &ele)
+{
+	if (!len) {
+		ele = NAN;
+		return true;
+	}
+
+	if (!readFloat(data, len, ele)) {
+		_errorString = "Invalid altitude";
+		return false;
+	}
+
+	return true;
+}
+
+bool NMEAParser::readGeoidHeight(const char *data, int len, qreal &gh)
+{
+	if (!len) {
+		gh = 0;
+		return true;
+	}
+
+	if (!readFloat(data, len, gh)) {
+		_errorString = "Invalid geoid height";
+		return false;
+	}
+
+	return true;
+}
+
+bool NMEAParser::readTime(const char *data, int len, QTime &time)
+{
+	int h, m, s, ms = 0;
+
+
+	if (!len) {
+		time = QTime();
+		return true;
+	}
+
+	if (len < 6)
+		goto error;
+
+	h = str2int(data, 2);
+	m = str2int(data + 2, 2);
+	s = str2int(data + 4, 2);
+	if (h < 0 || m < 0 || s < 0)
+		goto error;
+
+	if (len > 7 && data[6] == '.') {
+		if ((ms = str2int(data + 7, len - 7)) < 0)
+			goto error;
+	}
+
+	time = QTime(h, m, s, ms);
+	if (!time.isValid())
+		goto error;
+
+	return true;
+
+error:
+	_errorString = "Invalid time";
+	return false;
+}
+
+bool NMEAParser::readDate(const char *data, int len, QDate &date)
+{
+	int y, m, d;
+
+
+	if (!len) {
+		date = QDate();
+		return true;
+	}
+
+	if (len < 6)
+		goto error;
+
+	d = str2int(data, 2);
+	m = str2int(data + 2, 2);
+	y = str2int(data + 4, len - 4);
+	if (d < 0 || m < 0 || y < 0)
+		goto error;
+
+	if (len - 4 == 2)
+		date = QDate(y + 2000 < QDate::currentDate().year()
+		  ? 2000 + y : 1900 + y, m, d);
+	else
+		date = QDate(y, m, d);
+	if (!date.isValid())
+		goto error;
+
+	return true;
+
+error:
+	_errorString = "Invalid date";
+	return false;
+}
+
+bool NMEAParser::readLat(const char *data, int len, qreal &lat)
+{
+	int d, mi;
+	qreal mf;
+	bool ok;
+
+
+	if (!len) {
+		lat = NAN;
+		return true;
+	}
+
+	if (len < 7 || data[4] != '.')
+		goto error;
+
+	d = str2int(data, 2);
+	mi = str2int(data + 2, 2);
+	mf = QString(QByteArray::fromRawData(data + 4, len - 4)).toFloat(&ok);
+	if (d < 0 || mi < 0 || !ok)
+		goto error;
+
+	lat = d + (((qreal)mi + mf) / 60.0);
+	if (lat > 90)
+		goto error;
+
+	return true;
+
+error:
+	_errorString = "Invalid ltitude";
+	return false;
+}
+
+bool NMEAParser::readNS(const char *data, int len, qreal &lat)
+{
+	if (!len) {
+		lat = NAN;
+		return true;
+	}
+
+	if (len != 1 || !(*data == 'N' || *data == 'S')) {
+		_errorString = "Invalid N/S value";
+		return false;
+	}
+
+	if (*data == 'S')
+		lat = -lat;
+
+	return true;
+}
+
+bool NMEAParser::readLon(const char *data, int len, qreal &lon)
+{
+	int d, mi;
+	qreal mf;
+	bool ok;
+
+
+	if (!len) {
+		lon = NAN;
+		return true;
+	}
+
+	if (len < 8 || data[5] != '.')
+		goto error;
+
+	d = str2int(data, 3);
+	mi = str2int(data + 3, 2);
+	mf = QString(QByteArray::fromRawData(data + 5, len - 5)).toFloat(&ok);
+	if (d < 0 || mi < 0 || !ok)
+		goto error;
+
+	lon = d + (((qreal)mi + mf) / 60.0);
+	if (lon > 180)
+		goto error;
+
+	return true;
+
+error:
+	_errorString = "Invalid longitude";
+	return false;
+}
+
+bool NMEAParser::readEW(const char *data, int len, qreal &lon)
+{
+	if (!len) {
+		lon = NAN;
+		return true;
+	}
+
+	if (len != 1 || !(*data == 'E' || *data == 'W')) {
+		_errorString = "Invalid E/W value";
+		return false;
+	}
+
+	if (*data == 'W')
+		lon = -lon;
+
+	return true;
+}
+
 bool NMEAParser::readRMC(const char *line, int len)
 {
 	int col = 1;
 	const char *vp = line;
 	qreal lat, lon;
 	QTime time;
+	QDate date;
 	bool valid = true;
 
 	for (const char *lp = line; lp < line + len; lp++) {
 		if (*lp == ',' || *lp == '*') {
 			switch (col) {
 				case 1:
-					if (!readTime(vp, lp - vp, time)) {
-						_errorString = "Invalid time";
+					if (!readTime(vp, lp - vp, time))
 						return false;
-					}
 					break;
 				case 2:
 					if (*vp != 'A')
 						valid = false;
 					break;
 				case 3:
-					if (!readLat(vp, lp - vp, lat)) {
-						_errorString = "Invalid latitude";
+					if (!readLat(vp, lp - vp, lat))
 						return false;
-					}
 					break;
 				case 4:
-					if (!(*vp == 'N' || *vp == 'S')) {
-						_errorString = "Invalid latitude N|S";
+					if (!readNS(vp, lp - vp, lat))
 						return false;
-					}
-					if (*lp == 'S')
-						lat = -lat;
 					break;
 				case 5:
-					if (!readLon(vp, lp - vp, lon)) {
-						_errorString = "Invalid longitude";
+					if (!readLon(vp, lp - vp, lon))
 						return false;
-					}
 					break;
 				case 6:
-					if (!(*vp == 'E' || *vp == 'W')) {
-						_errorString = "Invalid latitude E|W";
+					if (!readEW(vp, lp - vp, lon))
 						return false;
-					}
-					if (*vp == 'W')
-						lon = -lon;
 					break;
 				case 9:
-					if (!readDate(vp, lp - vp, _date)) {
-						_errorString = "Invalid date";
+					if (!readDate(vp, lp - vp, date))
 						return false;
-					}
 					break;
 			}
 
@@ -184,9 +279,16 @@ bool NMEAParser::readRMC(const char *line, int len)
 		return false;
 	}
 
-	if (valid && !_GGA) {
+	if (!date.isNull()) {
+		if (_date.isNull() && !_time.isNull() && !_tracks.last().isEmpty())
+			_tracks.last().last().setTimestamp(QDateTime(date, _time, Qt::UTC));
+		_date = date;
+	}
+
+	if (valid && !_GGA && !std::isnan(lat) && !std::isnan(lon)) {
 		Trackpoint t(Coordinates(lon, lat));
-		t.setTimestamp(QDateTime(_date, time, Qt::UTC));
+		if (!_date.isNull() && !time.isNull())
+			t.setTimestamp(QDateTime(_date, time, Qt::UTC));
 		_tracks.last().append(t);
 	}
 
@@ -198,65 +300,46 @@ bool NMEAParser::readGGA(const char *line, int len)
 	int col = 1;
 	const char *vp = line;
 	qreal lat, lon, ele, gh;
-	QTime time;
 
 	for (const char *lp = line; lp < line + len; lp++) {
 		if (*lp == ',' || *lp == '*') {
 			switch (col) {
 				case 1:
-					if (!readTime(vp, lp - vp, time)) {
-						_errorString = "Invalid time";
+					if (!readTime(vp, lp - vp, _time))
 						return false;
-					}
 					break;
 				case 2:
-					if (!readLat(vp, lp - vp, lat)) {
-						_errorString = "Invalid latitude";
+					if (!readLat(vp, lp - vp, lat))
 						return false;
-					}
 					break;
 				case 3:
-					if (!(*vp == 'N' || *vp == 'S')) {
-						_errorString = "Invalid latitude N|S";
+					if (!readNS(vp, lp - vp, lon))
 						return false;
-					}
-					if (*vp == 'S')
-						lat = -lat;
 					break;
 				case 4:
-					if (!readLon(vp, lp - vp, lon)) {
-						_errorString = "Invalid longitude";
+					if (!readLon(vp, lp - vp, lon))
 						return false;
-					}
 					break;
 				case 5:
-					if (!(*vp == 'E' || *vp == 'W')) {
-						_errorString = "Invalid latitude E|W";
+					if (!readEW(vp, lp - vp, lon))
 						return false;
-					}
-					if (*vp == 'W')
-						lon = -lon;
 					break;
 				case 9:
-					if ((lp - vp) && !readFloat(vp, lp - vp, ele)) {
-						_errorString = "Invalid altitude";
+					if (!readAltitude(vp, lp - vp, ele))
 						return false;
-					}
 					break;
 				case 10:
-					if ((lp - vp) && *vp != 'M') {
+					if ((lp - vp) && !((lp - vp) == 1 && *vp == 'M')) {
 						_errorString = "Invalid altitude units";
 						return false;
 					}
 					break;
 				case 11:
-					if ((lp - vp) && !readFloat(vp, lp - vp, gh)) {
-						_errorString = "Invalid geoid height";
+					if (!readGeoidHeight(vp, lp - vp, gh))
 						return false;
-					}
 					break;
 				case 12:
-					if ((lp - vp) && *vp != 'M') {
+					if ((lp - vp) && !((lp - vp) == 1 && *vp == 'M')) {
 						_errorString = "Invalid geoid height units";
 						return false;
 					}
@@ -273,12 +356,16 @@ bool NMEAParser::readGGA(const char *line, int len)
 		return false;
 	}
 
-	_GGA = true;
+	if (!std::isnan(lat) && !std::isnan(lon)) {
+		Trackpoint t(Coordinates(lon, lat));
+		if (!(_time.isNull() || _date.isNull()))
+			t.setTimestamp(QDateTime(_date, _time, Qt::UTC));
+		if (!std::isnan(ele))
+			t.setElevation(ele - gh);
+		_tracks.last().append(t);
 
-	Trackpoint t(Coordinates(lon, lat));
-	t.setTimestamp(QDateTime(_date, time, Qt::UTC));
-	t.setElevation(ele - gh);
-	_tracks.last().append(t);
+		_GGA = true;
+	}
 
 	return true;
 }
@@ -294,32 +381,20 @@ bool NMEAParser::readWPL(const char *line, int len)
 		if (*lp == ',' || *lp == '*') {
 			switch (col) {
 				case 1:
-					if (!readLat(vp, lp - vp, lat)) {
-						_errorString = "Invalid latitude";
+					if (!readLat(vp, lp - vp, lat))
 						return false;
-					}
 					break;
 				case 2:
-					if (!(*vp == 'N' || *vp == 'S')) {
-						_errorString = "Invalid latitude N|S";
+					if (!readNS(vp, lp - vp, lat))
 						return false;
-					}
-					if (*vp == 'S')
-						lat = -lat;
 					break;
 				case 3:
-					if (!readLon(vp, lp - vp, lon)) {
-						_errorString = "Invalid longitude";
+					if (!readLon(vp, lp - vp, lon))
 						return false;
-					}
 					break;
 				case 4:
-					if (!(*vp == 'E' || *vp == 'W')) {
-						_errorString = "Invalid latitude E|W";
+					if (!readEW(vp, lp - vp, lon))
 						return false;
-					}
-					if (*vp == 'W')
-						lon = -lon;
 					break;
 				case 5:
 					name = QString(QByteArray(vp, lp - vp));
@@ -336,9 +411,11 @@ bool NMEAParser::readWPL(const char *line, int len)
 		return false;
 	}
 
-	Waypoint w(Coordinates(lon, lat));
-	w.setName(name);
-	_waypoints.append(w);
+	if (!std::isnan(lat) && !std::isnan(lon)) {
+		Waypoint w(Coordinates(lon, lat));
+		w.setName(name);
+		_waypoints.append(w);
+	}
 
 	return true;
 }
@@ -353,18 +430,24 @@ bool NMEAParser::readZDA(const char *line, int len)
 		if (*lp == ',' || *lp == '*') {
 			switch (col) {
 				case 2:
+					if (!(lp - vp))
+						return true;
 					if ((d = str2int(vp, lp - vp)) < 0) {
 						_errorString = "Invalid day";
 						return false;
 					}
 					break;
 				case 3:
+					if (!(lp - vp))
+						return true;
 					if ((m = str2int(vp, lp - vp)) < 0) {
 						_errorString = "Invalid month";
 						return false;
 					}
 					break;
 				case 4:
+					if (!(lp - vp))
+						return true;
 					if ((y = str2int(vp, lp - vp)) < 0) {
 						_errorString = "Invalid year";
 						return false;
@@ -395,13 +478,14 @@ bool NMEAParser::loadFile(QFile *file)
 {
 	qint64 len;
 	char line[80 + 2 + 1 + 1];
-	bool nmea = false;
 
 
 	_errorLine = 1;
 	_errorString.clear();
-
+	_date = QDate();
+	_time = QTime();
 	_GGA = false;
+
 	_tracks.append(TrackData());
 
 	while (!file->atEnd()) {
@@ -415,27 +499,20 @@ bool NMEAParser::loadFile(QFile *file)
 			return false;
 		}
 
-		if (!validSentence(line, len)) {
-			if (nmea)
-				fprintf(stderr, "%s:%d: Invalid NMEA sentence\n",
-				  qPrintable(file->fileName()), _errorLine);
-			_errorLine++;
-			continue;
-		} else
-			nmea = true;
-
-		if (!memcmp(line + 3, "RMC,", 4)) {
-			if (!readRMC(line + 7, len))
-				return false;
-		} else if (!memcmp(line + 3, "GGA,", 4)) {
-			if (!readGGA(line + 7, len))
-				return false;
-		} else if (!memcmp(line + 3, "WPL,", 4)) {
-			if (!readWPL(line + 7, len))
-				return false;
-		} else if (!memcmp(line + 3, "ZDA,", 4)) {
-			if (!readZDA(line + 7, len))
-				return false;
+		if (validSentence(line, len)) {
+			if (!memcmp(line + 3, "RMC,", 4)) {
+				if (!readRMC(line + 7, len))
+					return false;
+			} else if (!memcmp(line + 3, "GGA,", 4)) {
+				if (!readGGA(line + 7, len))
+					return false;
+			} else if (!memcmp(line + 3, "WPL,", 4)) {
+				if (!readWPL(line + 7, len))
+					return false;
+			} else if (!memcmp(line + 3, "ZDA,", 4)) {
+				if (!readZDA(line + 7, len))
+					return false;
+			}
 		}
 
 		_errorLine++;
diff --git a/src/nmeaparser.h b/src/nmeaparser.h
index 64a661ae..445c96d9 100644
--- a/src/nmeaparser.h
+++ b/src/nmeaparser.h
@@ -18,6 +18,15 @@ public:
 	int errorLine() const {return _errorLine;}
 
 private:
+	bool readEW(const char *data, int len, qreal &lon);
+	bool readLon(const char *data, int len, qreal &lon);
+	bool readNS(const char *data, int len, qreal &lat);
+	bool readLat(const char *data, int len, qreal &lat);
+	bool readDate(const char *data, int len, QDate &date);
+	bool readTime(const char *data, int len, QTime &time);
+	bool readAltitude(const char *data, int len, qreal &ele);
+	bool readGeoidHeight(const char *data, int len, qreal &gh);
+
 	bool readRMC(const char *line, int len);
 	bool readGGA(const char *line, int len);
 	bool readWPL(const char *line, int len);
@@ -27,6 +36,7 @@ private:
 	QString _errorString;
 
 	QDate _date;
+	QTime _time;
 	bool _GGA;
 };