mirror of
https://github.com/tumic0/GPXSee.git
synced 2024-11-24 11:45:53 +01:00
NMEA parser improvements
This commit is contained in:
parent
4e23df3a66
commit
2bdab0f449
@ -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++;
|
||||
|
||||
@ -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;
|
||||
};
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user