root/GPXSee
1
0
Fork 0
mirror of https://github.com/tumic0/GPXSee.git synced 2024-11-24 11:45:53 +01:00
Code Issues Projects Releases Activity

NMEA parser improvements

Browse Source
This commit is contained in:
Martin Tůma 2016-11-22 00:13:41 +01:00
parent 4e23df3a66
commit 2bdab0f449
2 changed files with 284 additions and 197 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

471
src/nmeaparser.cpp
View File

@ -3,104 +3,6 @@
#include "nmeaparser.h" #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) static bool validSentence(const char *line, int len)
{ {
const char *lp; const char *lp;
@ -117,60 +19,253 @@ static bool validSentence(const char *line, int len)
return true; 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) bool NMEAParser::readRMC(const char *line, int len)
{ {
int col = 1; int col = 1;
const char *vp = line; const char *vp = line;
qreal lat, lon; qreal lat, lon;
QTime time; QTime time;
QDate date;
bool valid = true; bool valid = true;
for (const char *lp = line; lp < line + len; lp++) { for (const char *lp = line; lp < line + len; lp++) {
if (*lp == ',' || *lp == '*') { if (*lp == ',' || *lp == '*') {
switch (col) { switch (col) {
case 1: case 1:
if (!readTime(vp, lp - vp, time)) { if (!readTime(vp, lp - vp, time))
_errorString = "Invalid time";
return false; return false;
}
break; break;
case 2: case 2:
if (*vp != 'A') if (*vp != 'A')
valid = false; valid = false;
break; break;
case 3: case 3:
if (!readLat(vp, lp - vp, lat)) { if (!readLat(vp, lp - vp, lat))
_errorString = "Invalid latitude";
return false; return false;
}
break; break;
case 4: case 4:
if (!(*vp == 'N' || *vp == 'S')) { if (!readNS(vp, lp - vp, lat))
_errorString = "Invalid latitude N|S";
return false; return false;
}
if (*lp == 'S')
lat = -lat;
break; break;
case 5: case 5:
if (!readLon(vp, lp - vp, lon)) { if (!readLon(vp, lp - vp, lon))
_errorString = "Invalid longitude";
return false; return false;
}
break; break;
case 6: case 6:
if (!(*vp == 'E' || *vp == 'W')) { if (!readEW(vp, lp - vp, lon))
_errorString = "Invalid latitude E|W";
return false; return false;
}
if (*vp == 'W')
lon = -lon;
break; break;
case 9: case 9:
if (!readDate(vp, lp - vp, _date)) { if (!readDate(vp, lp - vp, date))
_errorString = "Invalid date";
return false; return false;
}
break; break;
} }
@ -184,9 +279,16 @@ bool NMEAParser::readRMC(const char *line, int len)
return false; 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)); 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); _tracks.last().append(t);
} }
@ -198,65 +300,46 @@ bool NMEAParser::readGGA(const char *line, int len)
int col = 1; int col = 1;
const char *vp = line; const char *vp = line;
qreal lat, lon, ele, gh; qreal lat, lon, ele, gh;
QTime time;
for (const char *lp = line; lp < line + len; lp++) { for (const char *lp = line; lp < line + len; lp++) {
if (*lp == ',' || *lp == '*') { if (*lp == ',' || *lp == '*') {
switch (col) { switch (col) {
case 1: case 1:
if (!readTime(vp, lp - vp, time)) { if (!readTime(vp, lp - vp, _time))
_errorString = "Invalid time";
return false; return false;
}
break; break;
case 2: case 2:
if (!readLat(vp, lp - vp, lat)) { if (!readLat(vp, lp - vp, lat))
_errorString = "Invalid latitude";
return false; return false;
}
break; break;
case 3: case 3:
if (!(*vp == 'N' || *vp == 'S')) { if (!readNS(vp, lp - vp, lon))
_errorString = "Invalid latitude N|S";
return false; return false;
}
if (*vp == 'S')
lat = -lat;
break; break;
case 4: case 4:
if (!readLon(vp, lp - vp, lon)) { if (!readLon(vp, lp - vp, lon))
_errorString = "Invalid longitude";
return false; return false;
}
break; break;
case 5: case 5:
if (!(*vp == 'E' || *vp == 'W')) { if (!readEW(vp, lp - vp, lon))
_errorString = "Invalid latitude E|W";
return false; return false;
}
if (*vp == 'W')
lon = -lon;
break; break;
case 9: case 9:
if ((lp - vp) && !readFloat(vp, lp - vp, ele)) { if (!readAltitude(vp, lp - vp, ele))
_errorString = "Invalid altitude";
return false; return false;
}
break; break;
case 10: case 10:
if ((lp - vp) && *vp != 'M') { if ((lp - vp) && !((lp - vp) == 1 && *vp == 'M')) {
_errorString = "Invalid altitude units"; _errorString = "Invalid altitude units";
return false; return false;
} }
break; break;
case 11: case 11:
if ((lp - vp) && !readFloat(vp, lp - vp, gh)) { if (!readGeoidHeight(vp, lp - vp, gh))
_errorString = "Invalid geoid height";
return false; return false;
}
break; break;
case 12: case 12:
if ((lp - vp) && *vp != 'M') { if ((lp - vp) && !((lp - vp) == 1 && *vp == 'M')) {
_errorString = "Invalid geoid height units"; _errorString = "Invalid geoid height units";
return false; return false;
} }
@ -273,12 +356,16 @@ bool NMEAParser::readGGA(const char *line, int len)
return false; 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)); _GGA = true;
t.setTimestamp(QDateTime(_date, time, Qt::UTC)); }
t.setElevation(ele - gh);
_tracks.last().append(t);
return true; return true;
} }
@ -294,32 +381,20 @@ bool NMEAParser::readWPL(const char *line, int len)
if (*lp == ',' || *lp == '*') { if (*lp == ',' || *lp == '*') {
switch (col) { switch (col) {
case 1: case 1:
if (!readLat(vp, lp - vp, lat)) { if (!readLat(vp, lp - vp, lat))
_errorString = "Invalid latitude";
return false; return false;
}
break; break;
case 2: case 2:
if (!(*vp == 'N' || *vp == 'S')) { if (!readNS(vp, lp - vp, lat))
_errorString = "Invalid latitude N|S";
return false; return false;
}
if (*vp == 'S')
lat = -lat;
break; break;
case 3: case 3:
if (!readLon(vp, lp - vp, lon)) { if (!readLon(vp, lp - vp, lon))
_errorString = "Invalid longitude";
return false; return false;
}
break; break;
case 4: case 4:
if (!(*vp == 'E' || *vp == 'W')) { if (!readEW(vp, lp - vp, lon))
_errorString = "Invalid latitude E|W";
return false; return false;
}
if (*vp == 'W')
lon = -lon;
break; break;
case 5: case 5:
name = QString(QByteArray(vp, lp - vp)); name = QString(QByteArray(vp, lp - vp));
@ -336,9 +411,11 @@ bool NMEAParser::readWPL(const char *line, int len)
return false; return false;
} }
Waypoint w(Coordinates(lon, lat)); if (!std::isnan(lat) && !std::isnan(lon)) {
w.setName(name); Waypoint w(Coordinates(lon, lat));
_waypoints.append(w); w.setName(name);
_waypoints.append(w);
}
return true; return true;
} }
@ -353,18 +430,24 @@ bool NMEAParser::readZDA(const char *line, int len)
if (*lp == ',' || *lp == '*') { if (*lp == ',' || *lp == '*') {
switch (col) { switch (col) {
case 2: case 2:
if (!(lp - vp))
return true;
if ((d = str2int(vp, lp - vp)) < 0) { if ((d = str2int(vp, lp - vp)) < 0) {
_errorString = "Invalid day"; _errorString = "Invalid day";
return false; return false;
} }
break; break;
case 3: case 3:
if (!(lp - vp))
return true;
if ((m = str2int(vp, lp - vp)) < 0) { if ((m = str2int(vp, lp - vp)) < 0) {
_errorString = "Invalid month"; _errorString = "Invalid month";
return false; return false;
} }
break; break;
case 4: case 4:
if (!(lp - vp))
return true;
if ((y = str2int(vp, lp - vp)) < 0) { if ((y = str2int(vp, lp - vp)) < 0) {
_errorString = "Invalid year"; _errorString = "Invalid year";
return false; return false;
@ -395,13 +478,14 @@ bool NMEAParser::loadFile(QFile *file)
{ {
qint64 len; qint64 len;
char line[80 + 2 + 1 + 1]; char line[80 + 2 + 1 + 1];
bool nmea = false;
_errorLine = 1; _errorLine = 1;
_errorString.clear(); _errorString.clear();
_date = QDate();
_time = QTime();
_GGA = false; _GGA = false;
_tracks.append(TrackData()); _tracks.append(TrackData());
while (!file->atEnd()) { while (!file->atEnd()) {
@ -415,27 +499,20 @@ bool NMEAParser::loadFile(QFile *file)
return false; return false;
} }
if (!validSentence(line, len)) { if (validSentence(line, len)) {
if (nmea) if (!memcmp(line + 3, "RMC,", 4)) {
fprintf(stderr, "%s:%d: Invalid NMEA sentence\n", if (!readRMC(line + 7, len))
qPrintable(file->fileName()), _errorLine); return false;
_errorLine++; } else if (!memcmp(line + 3, "GGA,", 4)) {
continue; if (!readGGA(line + 7, len))
} else return false;
nmea = true; } else if (!memcmp(line + 3, "WPL,", 4)) {
if (!readWPL(line + 7, len))
if (!memcmp(line + 3, "RMC,", 4)) { return false;
if (!readRMC(line + 7, len)) } else if (!memcmp(line + 3, "ZDA,", 4)) {
return false; if (!readZDA(line + 7, len))
} else if (!memcmp(line + 3, "GGA,", 4)) { return false;
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++; _errorLine++;

10
src/nmeaparser.h
View File

@ -18,6 +18,15 @@ public:
int errorLine() const {return _errorLine;} int errorLine() const {return _errorLine;}
private: 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 readRMC(const char *line, int len);
bool readGGA(const char *line, int len); bool readGGA(const char *line, int len);
bool readWPL(const char *line, int len); bool readWPL(const char *line, int len);
@ -27,6 +36,7 @@ private:
QString _errorString; QString _errorString;
QDate _date; QDate _date;
QTime _time;
bool _GGA; bool _GGA;
}; };