mirror of
https://github.com/tumic0/GPXSee.git
synced 2024-12-01 07:01:16 +01:00
539 lines
9.5 KiB
C++
539 lines
9.5 KiB
C++
#include <cstring>
|
|
#include "common/util.h"
|
|
#include "nmeaparser.h"
|
|
|
|
|
|
static bool validSentence(const char *line, int len)
|
|
{
|
|
const char *lp;
|
|
|
|
if (len < 12 || line[0] != '$')
|
|
return false;
|
|
|
|
for (lp = line + len - 1; lp > line + 3; lp--)
|
|
if (!::isspace(*lp))
|
|
break;
|
|
if (*(lp-2) != '*' || !::isalnum(*(lp-1)) || !::isalnum(*(lp)))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
static bool readFloat(const char *data, int len, qreal &f)
|
|
{
|
|
bool ok;
|
|
|
|
f = 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 = Util::str2int(data, 2);
|
|
m = Util::str2int(data + 2, 2);
|
|
s = Util::str2int(data + 4, 2);
|
|
if (h < 0 || m < 0 || s < 0)
|
|
goto error;
|
|
|
|
if (len > 7 && data[6] == '.') {
|
|
if ((ms = Util::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 = Util::str2int(data, 2);
|
|
m = Util::str2int(data + 2, 2);
|
|
y = Util::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 = Util::str2int(data, 2);
|
|
mi = Util::str2int(data + 2, 2);
|
|
mf = 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 = Util::str2int(data, 3);
|
|
mi = Util::str2int(data + 3, 2);
|
|
mf = 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(CTX &ctx, const char *line, int len,
|
|
SegmentData &segment)
|
|
{
|
|
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))
|
|
return false;
|
|
break;
|
|
case 2:
|
|
if (*vp != 'A')
|
|
valid = false;
|
|
break;
|
|
case 3:
|
|
if (!readLat(vp, lp - vp, lat))
|
|
return false;
|
|
break;
|
|
case 4:
|
|
if (!readNS(vp, lp - vp, lat))
|
|
return false;
|
|
break;
|
|
case 5:
|
|
if (!readLon(vp, lp - vp, lon))
|
|
return false;
|
|
break;
|
|
case 6:
|
|
if (!readEW(vp, lp - vp, lon))
|
|
return false;
|
|
break;
|
|
case 9:
|
|
if (!readDate(vp, lp - vp, date))
|
|
return false;
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 9) {
|
|
_errorString = "Invalid RMC sentence";
|
|
return false;
|
|
}
|
|
|
|
if (!date.isNull()) {
|
|
if (ctx.date.isNull() && !ctx.time.isNull() && !segment.isEmpty())
|
|
segment.last().setTimestamp(QDateTime(date, ctx.time, Qt::UTC));
|
|
ctx.date = date;
|
|
}
|
|
|
|
Coordinates c(lon, lat);
|
|
if (valid && !ctx.GGA && c.isValid()) {
|
|
Trackpoint t(c);
|
|
if (!ctx.date.isNull() && !time.isNull())
|
|
t.setTimestamp(QDateTime(ctx.date, time, Qt::UTC));
|
|
segment.append(t);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::readGGA(CTX &ctx, const char *line, int len,
|
|
SegmentData &segment)
|
|
{
|
|
int col = 1;
|
|
const char *vp = line;
|
|
qreal lat, lon, ele, gh;
|
|
|
|
for (const char *lp = line; lp < line + len; lp++) {
|
|
if (*lp == ',' || *lp == '*') {
|
|
switch (col) {
|
|
case 1:
|
|
if (!readTime(vp, lp - vp, ctx.time))
|
|
return false;
|
|
break;
|
|
case 2:
|
|
if (!readLat(vp, lp - vp, lat))
|
|
return false;
|
|
break;
|
|
case 3:
|
|
if (!readNS(vp, lp - vp, lat))
|
|
return false;
|
|
break;
|
|
case 4:
|
|
if (!readLon(vp, lp - vp, lon))
|
|
return false;
|
|
break;
|
|
case 5:
|
|
if (!readEW(vp, lp - vp, lon))
|
|
return false;
|
|
break;
|
|
case 9:
|
|
if (!readAltitude(vp, lp - vp, ele))
|
|
return false;
|
|
break;
|
|
case 10:
|
|
if ((lp - vp) && !((lp - vp) == 1 && *vp == 'M')) {
|
|
_errorString = "Invalid altitude units";
|
|
return false;
|
|
}
|
|
break;
|
|
case 11:
|
|
if (!readGeoidHeight(vp, lp - vp, gh))
|
|
return false;
|
|
break;
|
|
case 12:
|
|
if ((lp - vp) && !((lp - vp) == 1 && *vp == 'M')) {
|
|
_errorString = "Invalid geoid height units";
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 12) {
|
|
_errorString = "Invalid GGA sentence";
|
|
return false;
|
|
}
|
|
|
|
Coordinates c(lon, lat);
|
|
if (c.isValid()) {
|
|
Trackpoint t(c);
|
|
if (!(ctx.time.isNull() || ctx.date.isNull()))
|
|
t.setTimestamp(QDateTime(ctx.date, ctx.time, Qt::UTC));
|
|
if (!std::isnan(ele))
|
|
t.setElevation(ele - gh);
|
|
segment.append(t);
|
|
|
|
ctx.GGA = true;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::readWPL(const char *line, int len, QVector<Waypoint> &waypoints)
|
|
{
|
|
int col = 1;
|
|
const char *vp = line;
|
|
qreal lat, lon;
|
|
QString name;
|
|
|
|
for (const char *lp = line; lp < line + len; lp++) {
|
|
if (*lp == ',' || *lp == '*') {
|
|
switch (col) {
|
|
case 1:
|
|
if (!readLat(vp, lp - vp, lat))
|
|
return false;
|
|
break;
|
|
case 2:
|
|
if (!readNS(vp, lp - vp, lat))
|
|
return false;
|
|
break;
|
|
case 3:
|
|
if (!readLon(vp, lp - vp, lon))
|
|
return false;
|
|
break;
|
|
case 4:
|
|
if (!readEW(vp, lp - vp, lon))
|
|
return false;
|
|
break;
|
|
case 5:
|
|
name = QString(QByteArray(vp, lp - vp));
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 4) {
|
|
_errorString = "Invalid WPL sentence";
|
|
return false;
|
|
}
|
|
|
|
Coordinates c(lon, lat);
|
|
if (c.isValid()) {
|
|
Waypoint w(c);
|
|
w.setName(name);
|
|
waypoints.append(w);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::readZDA(CTX &ctx, const char *line, int len)
|
|
{
|
|
int col = 1;
|
|
const char *vp = line;
|
|
int d, m, y;
|
|
|
|
for (const char *lp = line; lp < line + len; lp++) {
|
|
if (*lp == ',' || *lp == '*') {
|
|
switch (col) {
|
|
case 2:
|
|
if (!(lp - vp))
|
|
return true;
|
|
if ((d = Util::str2int(vp, lp - vp)) < 0) {
|
|
_errorString = "Invalid day";
|
|
return false;
|
|
}
|
|
break;
|
|
case 3:
|
|
if (!(lp - vp))
|
|
return true;
|
|
if ((m = Util::str2int(vp, lp - vp)) < 0) {
|
|
_errorString = "Invalid month";
|
|
return false;
|
|
}
|
|
break;
|
|
case 4:
|
|
if (!(lp - vp))
|
|
return true;
|
|
if ((y = Util::str2int(vp, lp - vp)) < 0) {
|
|
_errorString = "Invalid year";
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 4) {
|
|
_errorString = "Invalid ZDA sentence";
|
|
return false;
|
|
}
|
|
|
|
ctx.date = QDate(y, m, d);
|
|
if (!ctx.date.isValid()) {
|
|
_errorString = "Invalid date";
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::parse(QFile *file, QList<TrackData> &tracks,
|
|
QList<RouteData> &routes, QList<Area> &polygons,
|
|
QVector<Waypoint> &waypoints)
|
|
{
|
|
Q_UNUSED(routes);
|
|
Q_UNUSED(polygons);
|
|
qint64 len;
|
|
char line[80 + 2 + 1 + 1];
|
|
SegmentData segment;
|
|
CTX ctx;
|
|
|
|
|
|
_errorLine = 1;
|
|
_errorString.clear();
|
|
|
|
while (!file->atEnd()) {
|
|
len = file->readLine(line, sizeof(line));
|
|
|
|
if (len < 0) {
|
|
_errorString = "I/O error";
|
|
return false;
|
|
} else if (len > (qint64)sizeof(line) - 1) {
|
|
_errorString = "Line limit exceeded";
|
|
return false;
|
|
}
|
|
|
|
if (validSentence(line, len)) {
|
|
if (!memcmp(line + 3, "RMC,", 4)) {
|
|
if (!readRMC(ctx, line + 7, len - 7, segment))
|
|
return false;
|
|
} else if (!memcmp(line + 3, "GGA,", 4)) {
|
|
if (!readGGA(ctx, line + 7, len - 7, segment))
|
|
return false;
|
|
} else if (!memcmp(line + 3, "WPL,", 4)) {
|
|
if (!readWPL(line + 7, len - 7, waypoints))
|
|
return false;
|
|
} else if (!memcmp(line + 3, "ZDA,", 4)) {
|
|
if (!readZDA(ctx, line + 7, len - 7))
|
|
return false;
|
|
}
|
|
}
|
|
|
|
_errorLine++;
|
|
}
|
|
|
|
if (!segment.size() && !waypoints.size()) {
|
|
_errorString = "No usable NMEA sentence found";
|
|
return false;
|
|
}
|
|
|
|
if (segment.size()) {
|
|
tracks.append(TrackData());
|
|
tracks.last().append(segment);
|
|
}
|
|
|
|
return true;
|
|
}
|