mirror of
https://github.com/tumic0/GPXSee.git
synced 2024-10-07 07:13:21 +02:00
451 lines
8.3 KiB
C++
451 lines
8.3 KiB
C++
#include <cstring>
|
|
#include "misc.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)
|
|
{
|
|
const char *lp;
|
|
|
|
if (len < 10 || line[0] != '$')
|
|
return false;
|
|
|
|
for (lp = line + len - 1; lp > line; lp--)
|
|
if (!::isspace(*lp))
|
|
break;
|
|
if (*(lp-2) != '*' || !::isalnum(*(lp-1)) || !::isalnum(*(lp)))
|
|
return false;
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::readRMC(const char *line, int len)
|
|
{
|
|
int col = 1;
|
|
const char *vp = line;
|
|
qreal lat, lon;
|
|
QTime time;
|
|
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";
|
|
return false;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (*vp != 'A')
|
|
valid = false;
|
|
break;
|
|
case 3:
|
|
if (!readLat(vp, lp - vp, lat)) {
|
|
_errorString = "Invalid latitude";
|
|
return false;
|
|
}
|
|
break;
|
|
case 4:
|
|
if (!(*vp == 'N' || *vp == 'S')) {
|
|
_errorString = "Invalid latitude N|S";
|
|
return false;
|
|
}
|
|
if (*lp == 'S')
|
|
lat = -lat;
|
|
break;
|
|
case 5:
|
|
if (!readLon(vp, lp - vp, lon)) {
|
|
_errorString = "Invalid longitude";
|
|
return false;
|
|
}
|
|
break;
|
|
case 6:
|
|
if (!(*vp == 'E' || *vp == 'W')) {
|
|
_errorString = "Invalid latitude E|W";
|
|
return false;
|
|
}
|
|
if (*vp == 'W')
|
|
lon = -lon;
|
|
break;
|
|
case 9:
|
|
if (!readDate(vp, lp - vp, _date)) {
|
|
_errorString = "Invalid date";
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 9) {
|
|
_errorString = "Invalid RMC sentence";
|
|
return false;
|
|
}
|
|
|
|
if (valid && !_GGA) {
|
|
Trackpoint t(Coordinates(lon, lat));
|
|
t.setTimestamp(QDateTime(_date, time, Qt::UTC));
|
|
_tracks.last().append(t);
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
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";
|
|
return false;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (!readLat(vp, lp - vp, lat)) {
|
|
_errorString = "Invalid latitude";
|
|
return false;
|
|
}
|
|
break;
|
|
case 3:
|
|
if (!(*vp == 'N' || *vp == 'S')) {
|
|
_errorString = "Invalid latitude N|S";
|
|
return false;
|
|
}
|
|
if (*vp == 'S')
|
|
lat = -lat;
|
|
break;
|
|
case 4:
|
|
if (!readLon(vp, lp - vp, lon)) {
|
|
_errorString = "Invalid longitude";
|
|
return false;
|
|
}
|
|
break;
|
|
case 5:
|
|
if (!(*vp == 'E' || *vp == 'W')) {
|
|
_errorString = "Invalid latitude E|W";
|
|
return false;
|
|
}
|
|
if (*vp == 'W')
|
|
lon = -lon;
|
|
break;
|
|
case 9:
|
|
if ((lp - vp) && !readFloat(vp, lp - vp, ele)) {
|
|
_errorString = "Invalid altitude";
|
|
return false;
|
|
}
|
|
break;
|
|
case 10:
|
|
if ((lp - vp) && *vp != 'M') {
|
|
_errorString = "Invalid altitude units";
|
|
return false;
|
|
}
|
|
break;
|
|
case 11:
|
|
if ((lp - vp) && !readFloat(vp, lp - vp, gh)) {
|
|
_errorString = "Invalid geoid height";
|
|
return false;
|
|
}
|
|
break;
|
|
case 12:
|
|
if ((lp - vp) && *vp != 'M') {
|
|
_errorString = "Invalid geoid height units";
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 12) {
|
|
_errorString = "Invalid GGA sentence";
|
|
return false;
|
|
}
|
|
|
|
_GGA = true;
|
|
|
|
Trackpoint t(Coordinates(lon, lat));
|
|
t.setTimestamp(QDateTime(_date, time, Qt::UTC));
|
|
t.setElevation(ele - gh);
|
|
_tracks.last().append(t);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::readWPL(const char *line, int len)
|
|
{
|
|
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)) {
|
|
_errorString = "Invalid latitude";
|
|
return false;
|
|
}
|
|
break;
|
|
case 2:
|
|
if (!(*vp == 'N' || *vp == 'S')) {
|
|
_errorString = "Invalid latitude N|S";
|
|
return false;
|
|
}
|
|
if (*vp == 'S')
|
|
lat = -lat;
|
|
break;
|
|
case 3:
|
|
if (!readLon(vp, lp - vp, lon)) {
|
|
_errorString = "Invalid longitude";
|
|
return false;
|
|
}
|
|
break;
|
|
case 4:
|
|
if (!(*vp == 'E' || *vp == 'W')) {
|
|
_errorString = "Invalid latitude E|W";
|
|
return false;
|
|
}
|
|
if (*vp == 'W')
|
|
lon = -lon;
|
|
break;
|
|
case 5:
|
|
name = QString(QByteArray(vp, lp - vp));
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 4) {
|
|
_errorString = "Invalid WPL sentence";
|
|
return false;
|
|
}
|
|
|
|
Waypoint w(Coordinates(lon, lat));
|
|
w.setName(name);
|
|
_waypoints.append(w);
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::readZDA(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 ((d = str2int(vp, lp - vp)) < 0) {
|
|
_errorString = "Invalid day";
|
|
return false;
|
|
}
|
|
break;
|
|
case 3:
|
|
if ((m = str2int(vp, lp - vp)) < 0) {
|
|
_errorString = "Invalid month";
|
|
return false;
|
|
}
|
|
break;
|
|
case 4:
|
|
if ((y = str2int(vp, lp - vp)) < 0) {
|
|
_errorString = "Invalid year";
|
|
return false;
|
|
}
|
|
break;
|
|
}
|
|
|
|
col++;
|
|
vp = lp + 1;
|
|
}
|
|
}
|
|
|
|
if (col < 4) {
|
|
_errorString = "Invalid ZDA sentence";
|
|
return false;
|
|
}
|
|
|
|
_date = QDate(y, m, d);
|
|
if (!_date.isValid()) {
|
|
_errorString = "Invalid date";
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|
|
|
|
bool NMEAParser::loadFile(QFile *file)
|
|
{
|
|
qint64 len;
|
|
char line[80 + 2 + 1 + 1];
|
|
bool nmea = false;
|
|
|
|
|
|
_errorLine = 1;
|
|
_errorString.clear();
|
|
|
|
_GGA = false;
|
|
_tracks.append(TrackData());
|
|
|
|
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 (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;
|
|
}
|
|
|
|
_errorLine++;
|
|
}
|
|
|
|
if (!_tracks.last().size() && !_waypoints.size()) {
|
|
_errorString = "No usable NMEA sentence found";
|
|
return false;
|
|
}
|
|
|
|
return true;
|
|
}
|