/**************************************************************************** ** ** Copyright (C) 2016 Jolla Ltd. ** Contact: Aaron McCarthy ** Copyright (C) 2016 The Qt Company Ltd. ** Contact: https://www.qt.io/licensing/ ** ** This file is part of the QtPositioning module of the Qt Toolkit. ** ** $QT_BEGIN_LICENSE:LGPL$ ** Commercial License Usage ** Licensees holding valid commercial Qt licenses may use this file in ** accordance with the commercial license agreement provided with the ** Software or, alternatively, in accordance with the terms contained in ** a written agreement between you and The Qt Company. For licensing terms ** and conditions see https://www.qt.io/terms-conditions. For further ** information use the contact form at https://www.qt.io/contact-us. ** ** GNU Lesser General Public License Usage ** Alternatively, this file may be used under the terms of the GNU Lesser ** General Public License version 3 as published by the Free Software ** Foundation and appearing in the file LICENSE.LGPL3 included in the ** packaging of this file. Please review the following information to ** ensure the GNU Lesser General Public License version 3 requirements ** will be met: https://www.gnu.org/licenses/lgpl-3.0.html. ** ** GNU General Public License Usage ** Alternatively, this file may be used under the terms of the GNU ** General Public License version 2.0 or (at your option) the GNU General ** Public license version 3 or any later version approved by the KDE Free ** Qt Foundation. The licenses are as published by the Free Software ** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3 ** included in the packaging of this file. Please review the following ** information to ensure the GNU General Public License requirements will ** be met: https://www.gnu.org/licenses/gpl-2.0.html and ** https://www.gnu.org/licenses/gpl-3.0.html. ** ** $QT_END_LICENSE$ ** ****************************************************************************/ #include "qlocationutils_p.h" #include "qgeopositioninfo.h" #include #include #include #include #include QT_BEGIN_NAMESPACE // converts e.g. 15306.0235 from NMEA sentence to 153.100392 static double qlocationutils_nmeaDegreesToDecimal(double nmeaDegrees) { double deg; double min = 100.0 * modf(nmeaDegrees / 100.0, °); return deg + (min / 60.0); } static void qlocationutils_readGga(const char *data, int size, QGeoPositionInfo *info, double uere, bool *hasFix) { QByteArray sentence(data, size); QList parts = sentence.split(','); QGeoCoordinate coord; if (hasFix && parts.count() > 6 && parts[6].count() > 0) *hasFix = parts[6].toInt() > 0; if (parts.count() > 1 && parts[1].count() > 0) { QTime time; if (QLocationUtils::getNmeaTime(parts[1], &time)) info->setTimestamp(QDateTime(QDate(), time, Qt::UTC)); } if (parts.count() > 5 && parts[3].count() == 1 && parts[5].count() == 1) { double lat; double lng; if (QLocationUtils::getNmeaLatLong(parts[2], parts[3][0], parts[4], parts[5][0], &lat, &lng)) { coord.setLatitude(lat); coord.setLongitude(lng); } } if (parts.count() > 8 && !parts[8].isEmpty()) { bool hasHdop = false; double hdop = parts[8].toDouble(&hasHdop); if (hasHdop) info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere); } if (parts.count() > 9 && parts[9].count() > 0) { bool hasAlt = false; double alt = parts[9].toDouble(&hasAlt); if (hasAlt) coord.setAltitude(alt); } if (coord.type() != QGeoCoordinate::InvalidCoordinate) info->setCoordinate(coord); } static void qlocationutils_readGsa(const char *data, int size, QGeoPositionInfo *info, double uere, bool *hasFix) { QList parts = QByteArray::fromRawData(data, size).split(','); if (hasFix && parts.count() > 2 && !parts[2].isEmpty()) *hasFix = parts[2].toInt() > 0; if (parts.count() > 16 && !parts[16].isEmpty()) { bool hasHdop = false; double hdop = parts[16].toDouble(&hasHdop); if (hasHdop) info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere); } if (parts.count() > 17 && !parts[17].isEmpty()) { bool hasVdop = false; double vdop = parts[17].toDouble(&hasVdop); if (hasVdop) info->setAttribute(QGeoPositionInfo::VerticalAccuracy, 2 * vdop * uere); } } static void qlocationutils_readGll(const char *data, int size, QGeoPositionInfo *info, bool *hasFix) { QByteArray sentence(data, size); QList parts = sentence.split(','); QGeoCoordinate coord; if (hasFix && parts.count() > 6 && parts[6].count() > 0) *hasFix = (parts[6][0] == 'A'); if (parts.count() > 5 && parts[5].count() > 0) { QTime time; if (QLocationUtils::getNmeaTime(parts[5], &time)) info->setTimestamp(QDateTime(QDate(), time, Qt::UTC)); } if (parts.count() > 4 && parts[2].count() == 1 && parts[4].count() == 1) { double lat; double lng; if (QLocationUtils::getNmeaLatLong(parts[1], parts[2][0], parts[3], parts[4][0], &lat, &lng)) { coord.setLatitude(lat); coord.setLongitude(lng); } } if (coord.type() != QGeoCoordinate::InvalidCoordinate) info->setCoordinate(coord); } static void qlocationutils_readRmc(const char *data, int size, QGeoPositionInfo *info, bool *hasFix) { QByteArray sentence(data, size); QList parts = sentence.split(','); QGeoCoordinate coord; QDate date; QTime time; if (hasFix && parts.count() > 2 && parts[2].count() > 0) *hasFix = (parts[2][0] == 'A'); if (parts.count() > 9 && parts[9].count() == 6) { date = QDate::fromString(QString::fromLatin1(parts[9]), QStringLiteral("ddMMyy")); if (date.isValid()) date = date.addYears(100); // otherwise starts from 1900 else date = QDate(); } if (parts.count() > 1 && parts[1].count() > 0) QLocationUtils::getNmeaTime(parts[1], &time); if (parts.count() > 6 && parts[4].count() == 1 && parts[6].count() == 1) { double lat; double lng; if (QLocationUtils::getNmeaLatLong(parts[3], parts[4][0], parts[5], parts[6][0], &lat, &lng)) { coord.setLatitude(lat); coord.setLongitude(lng); } } bool parsed = false; double value = 0.0; if (parts.count() > 7 && parts[7].count() > 0) { value = parts[7].toDouble(&parsed); if (parsed) info->setAttribute(QGeoPositionInfo::GroundSpeed, qreal(value * 1.852 / 3.6)); // knots -> m/s } if (parts.count() > 8 && parts[8].count() > 0) { value = parts[8].toDouble(&parsed); if (parsed) info->setAttribute(QGeoPositionInfo::Direction, qreal(value)); } if (parts.count() > 11 && parts[11].count() == 1 && (parts[11][0] == 'E' || parts[11][0] == 'W')) { value = parts[10].toDouble(&parsed); if (parsed) { if (parts[11][0] == 'W') value *= -1; info->setAttribute(QGeoPositionInfo::MagneticVariation, qreal(value)); } } if (coord.type() != QGeoCoordinate::InvalidCoordinate) info->setCoordinate(coord); info->setTimestamp(QDateTime(date, time, Qt::UTC)); } static void qlocationutils_readVtg(const char *data, int size, QGeoPositionInfo *info, bool *hasFix) { if (hasFix) *hasFix = false; QByteArray sentence(data, size); QList parts = sentence.split(','); bool parsed = false; double value = 0.0; if (parts.count() > 1 && parts[1].count() > 0) { value = parts[1].toDouble(&parsed); if (parsed) info->setAttribute(QGeoPositionInfo::Direction, qreal(value)); } if (parts.count() > 7 && parts[7].count() > 0) { value = parts[7].toDouble(&parsed); if (parsed) info->setAttribute(QGeoPositionInfo::GroundSpeed, qreal(value / 3.6)); // km/h -> m/s } } static void qlocationutils_readZda(const char *data, int size, QGeoPositionInfo *info, bool *hasFix) { if (hasFix) *hasFix = false; QByteArray sentence(data, size); QList parts = sentence.split(','); QDate date; QTime time; if (parts.count() > 1 && parts[1].count() > 0) QLocationUtils::getNmeaTime(parts[1], &time); if (parts.count() > 4 && parts[2].count() > 0 && parts[3].count() > 0 && parts[4].count() == 4) { // must be full 4-digit year int day = parts[2].toUInt(); int month = parts[3].toUInt(); int year = parts[4].toUInt(); if (day > 0 && month > 0 && year > 0) date.setDate(year, month, day); } info->setTimestamp(QDateTime(date, time, Qt::UTC)); } QLocationUtils::NmeaSentence QLocationUtils::getNmeaSentenceType(const char *data, int size) { if (size < 6 || data[0] != '$' || !hasValidNmeaChecksum(data, size)) return NmeaSentenceInvalid; if (data[3] == 'G' && data[4] == 'G' && data[5] == 'A') return NmeaSentenceGGA; if (data[3] == 'G' && data[4] == 'S' && data[5] == 'A') return NmeaSentenceGSA; if (data[3] == 'G' && data[4] == 'L' && data[5] == 'L') return NmeaSentenceGLL; if (data[3] == 'R' && data[4] == 'M' && data[5] == 'C') return NmeaSentenceRMC; if (data[3] == 'V' && data[4] == 'T' && data[5] == 'G') return NmeaSentenceVTG; if (data[3] == 'Z' && data[4] == 'D' && data[5] == 'A') return NmeaSentenceZDA; return NmeaSentenceInvalid; } bool QLocationUtils::getPosInfoFromNmea(const char *data, int size, QGeoPositionInfo *info, double uere, bool *hasFix) { if (!info) return false; if (hasFix) *hasFix = false; NmeaSentence nmeaType = getNmeaSentenceType(data, size); if (nmeaType == NmeaSentenceInvalid) return false; // Adjust size so that * and following characters are not parsed by the following functions. for (int i = 0; i < size; ++i) { if (data[i] == '*') { size = i; break; } } switch (nmeaType) { case NmeaSentenceGGA: qlocationutils_readGga(data, size, info, uere, hasFix); return true; case NmeaSentenceGSA: qlocationutils_readGsa(data, size, info, uere, hasFix); return true; case NmeaSentenceGLL: qlocationutils_readGll(data, size, info, hasFix); return true; case NmeaSentenceRMC: qlocationutils_readRmc(data, size, info, hasFix); return true; case NmeaSentenceVTG: qlocationutils_readVtg(data, size, info, hasFix); return true; case NmeaSentenceZDA: qlocationutils_readZda(data, size, info, hasFix); return true; default: return false; } } bool QLocationUtils::hasValidNmeaChecksum(const char *data, int size) { int asteriskIndex = -1; for (int i = 0; i < size; ++i) { if (data[i] == '*') { asteriskIndex = i; break; } } const int CSUM_LEN = 2; if (asteriskIndex < 0 || asteriskIndex + CSUM_LEN >= size) return false; // XOR byte value of all characters between '$' and '*' int result = 0; for (int i = 1; i < asteriskIndex; ++i) result ^= data[i]; /* char calc[CSUM_LEN + 1]; ::snprintf(calc, CSUM_LEN + 1, "%02x", result); return ::strncmp(calc, &data[asteriskIndex+1], 2) == 0; */ QByteArray checkSumBytes(&data[asteriskIndex + 1], 2); bool ok = false; int checksum = checkSumBytes.toInt(&ok,16); return ok && checksum == result; } bool QLocationUtils::getNmeaTime(const QByteArray &bytes, QTime *time) { int dotIndex = bytes.indexOf('.'); QTime tempTime; if (dotIndex < 0) { tempTime = QTime::fromString(QString::fromLatin1(bytes.constData()), QStringLiteral("hhmmss")); } else { tempTime = QTime::fromString(QString::fromLatin1(bytes.mid(0, dotIndex)), QStringLiteral("hhmmss")); bool hasMsecs = false; int midLen = qMin(3, bytes.size() - dotIndex - 1); int msecs = bytes.mid(dotIndex + 1, midLen).toUInt(&hasMsecs); if (hasMsecs) tempTime = tempTime.addMSecs(msecs*(midLen == 3 ? 1 : midLen == 2 ? 10 : 100)); } if (tempTime.isValid()) { *time = tempTime; return true; } return false; } bool QLocationUtils::getNmeaLatLong(const QByteArray &latString, char latDirection, const QByteArray &lngString, char lngDirection, double *lat, double *lng) { if ((latDirection != 'N' && latDirection != 'S') || (lngDirection != 'E' && lngDirection != 'W')) { return false; } bool hasLat = false; bool hasLong = false; double tempLat = latString.toDouble(&hasLat); double tempLng = lngString.toDouble(&hasLong); if (hasLat && hasLong) { tempLat = qlocationutils_nmeaDegreesToDecimal(tempLat); if (latDirection == 'S') tempLat *= -1; tempLng = qlocationutils_nmeaDegreesToDecimal(tempLng); if (lngDirection == 'W') tempLng *= -1; if (isValidLat(tempLat) && isValidLong(tempLng)) { *lat = tempLat; *lng = tempLng; return true; } } return false; } QT_END_NAMESPACE