1/****************************************************************************
2**
3** Copyright (C) 2016 Jolla Ltd.
4** Contact: Aaron McCarthy <aaron.mccarthy@jollamobile.com>
5** Copyright (C) 2016 The Qt Company Ltd.
6** Contact: https://www.qt.io/licensing/
7**
8** This file is part of the QtPositioning module of the Qt Toolkit.
9**
10** $QT_BEGIN_LICENSE:LGPL$
11** Commercial License Usage
12** Licensees holding valid commercial Qt licenses may use this file in
13** accordance with the commercial license agreement provided with the
14** Software or, alternatively, in accordance with the terms contained in
15** a written agreement between you and The Qt Company. For licensing terms
16** and conditions see https://www.qt.io/terms-conditions. For further
17** information use the contact form at https://www.qt.io/contact-us.
18**
19** GNU Lesser General Public License Usage
20** Alternatively, this file may be used under the terms of the GNU Lesser
21** General Public License version 3 as published by the Free Software
22** Foundation and appearing in the file LICENSE.LGPL3 included in the
23** packaging of this file. Please review the following information to
24** ensure the GNU Lesser General Public License version 3 requirements
25** will be met: https://www.gnu.org/licenses/lgpl-3.0.html.
26**
27** GNU General Public License Usage
28** Alternatively, this file may be used under the terms of the GNU
29** General Public License version 2.0 or (at your option) the GNU General
30** Public license version 3 or any later version approved by the KDE Free
31** Qt Foundation. The licenses are as published by the Free Software
32** Foundation and appearing in the file LICENSE.GPL2 and LICENSE.GPL3
33** included in the packaging of this file. Please review the following
34** information to ensure the GNU General Public License requirements will
35** be met: https://www.gnu.org/licenses/gpl-2.0.html and
36** https://www.gnu.org/licenses/gpl-3.0.html.
37**
38** $QT_END_LICENSE$
39**
40****************************************************************************/
41#include "qlocationutils_p.h"
42#include "qgeopositioninfo.h"
43#include "qgeosatelliteinfo.h"
44
45#include <QTime>
46#include <QList>
47#include <QByteArray>
48#include <QDebug>
49
50#include <math.h>
51
52QT_BEGIN_NAMESPACE
53
54// converts e.g. 15306.0235 from NMEA sentence to 153.100392
55static double qlocationutils_nmeaDegreesToDecimal(double nmeaDegrees)
56{
57 double deg;
58 double min = 100.0 * modf(x: nmeaDegrees / 100.0, iptr: &deg);
59 return deg + (min / 60.0);
60}
61
62static void qlocationutils_readGga(const char *data, int size, QGeoPositionInfo *info, double uere,
63 bool *hasFix)
64{
65 QByteArray sentence(data, size);
66 QList<QByteArray> parts = sentence.split(sep: ',');
67 QGeoCoordinate coord;
68
69 if (hasFix && parts.count() > 6 && parts[6].count() > 0)
70 *hasFix = parts[6].toInt() > 0;
71
72 if (parts.count() > 1 && parts[1].count() > 0) {
73 QTime time;
74 if (QLocationUtils::getNmeaTime(bytes: parts[1], time: &time))
75 info->setTimestamp(QDateTime(QDate(), time, Qt::UTC));
76 }
77
78 if (parts.count() > 5 && parts[3].count() == 1 && parts[5].count() == 1) {
79 double lat;
80 double lng;
81 if (QLocationUtils::getNmeaLatLong(latString: parts[2], latDirection: parts[3][0], lngString: parts[4], lngDirection: parts[5][0], lat: &lat, lon: &lng)) {
82 coord.setLatitude(lat);
83 coord.setLongitude(lng);
84 }
85 }
86
87 if (parts.count() > 8 && !parts[8].isEmpty()) {
88 bool hasHdop = false;
89 double hdop = parts[8].toDouble(ok: &hasHdop);
90 if (hasHdop)
91 info->setAttribute(attribute: QGeoPositionInfo::HorizontalAccuracy, value: 2 * hdop * uere);
92 }
93
94 if (parts.count() > 9 && parts[9].count() > 0) {
95 bool hasAlt = false;
96 double alt = parts[9].toDouble(ok: &hasAlt);
97 if (hasAlt)
98 coord.setAltitude(alt);
99 }
100
101 if (coord.type() != QGeoCoordinate::InvalidCoordinate)
102 info->setCoordinate(coord);
103}
104
105static void qlocationutils_readGsa(const char *data, int size, QGeoPositionInfo *info, double uere,
106 bool *hasFix)
107{
108 QList<QByteArray> parts = QByteArray::fromRawData(data, size).split(sep: ',');
109
110 if (hasFix && parts.count() > 2 && !parts[2].isEmpty())
111 *hasFix = parts[2].toInt() > 0;
112
113 if (parts.count() > 16 && !parts[16].isEmpty()) {
114 bool hasHdop = false;
115 double hdop = parts[16].toDouble(ok: &hasHdop);
116 if (hasHdop)
117 info->setAttribute(attribute: QGeoPositionInfo::HorizontalAccuracy, value: 2 * hdop * uere);
118 }
119
120 if (parts.count() > 17 && !parts[17].isEmpty()) {
121 bool hasVdop = false;
122 double vdop = parts[17].toDouble(ok: &hasVdop);
123 if (hasVdop)
124 info->setAttribute(attribute: QGeoPositionInfo::VerticalAccuracy, value: 2 * vdop * uere);
125 }
126}
127
128static void qlocationutils_readGsa(const char *data,
129 int size,
130 QList<int> &pnrsInUse)
131{
132 QList<QByteArray> parts = QByteArray::fromRawData(data, size).split(sep: ',');
133 pnrsInUse.clear();
134 if (parts.count() <= 2)
135 return;
136 bool ok;
137 for (int i = 3; i <= qMin(a: 14, b: parts.size()); ++i) {
138 const QByteArray &pnrString = parts.at(i);
139 if (pnrString.isEmpty())
140 continue;
141 int pnr = pnrString.toInt(ok: &ok);
142 if (ok)
143 pnrsInUse.append(t: pnr);
144 }
145}
146
147static void qlocationutils_readGll(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
148{
149 QByteArray sentence(data, size);
150 QList<QByteArray> parts = sentence.split(sep: ',');
151 QGeoCoordinate coord;
152
153 if (hasFix && parts.count() > 6 && parts[6].count() > 0)
154 *hasFix = (parts[6][0] == 'A');
155
156 if (parts.count() > 5 && parts[5].count() > 0) {
157 QTime time;
158 if (QLocationUtils::getNmeaTime(bytes: parts[5], time: &time))
159 info->setTimestamp(QDateTime(QDate(), time, Qt::UTC));
160 }
161
162 if (parts.count() > 4 && parts[2].count() == 1 && parts[4].count() == 1) {
163 double lat;
164 double lng;
165 if (QLocationUtils::getNmeaLatLong(latString: parts[1], latDirection: parts[2][0], lngString: parts[3], lngDirection: parts[4][0], lat: &lat, lon: &lng)) {
166 coord.setLatitude(lat);
167 coord.setLongitude(lng);
168 }
169 }
170
171 if (coord.type() != QGeoCoordinate::InvalidCoordinate)
172 info->setCoordinate(coord);
173}
174
175static void qlocationutils_readRmc(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
176{
177 QByteArray sentence(data, size);
178 QList<QByteArray> parts = sentence.split(sep: ',');
179 QGeoCoordinate coord;
180 QDate date;
181 QTime time;
182
183 if (hasFix && parts.count() > 2 && parts[2].count() > 0)
184 *hasFix = (parts[2][0] == 'A');
185
186 if (parts.count() > 9 && parts[9].count() == 6) {
187 date = QDate::fromString(s: QString::fromLatin1(str: parts[9]), QStringLiteral("ddMMyy"));
188 if (date.isValid())
189 date = date.addYears(years: 100); // otherwise starts from 1900
190 else
191 date = QDate();
192 }
193
194 if (parts.count() > 1 && parts[1].count() > 0)
195 QLocationUtils::getNmeaTime(bytes: parts[1], time: &time);
196
197 if (parts.count() > 6 && parts[4].count() == 1 && parts[6].count() == 1) {
198 double lat;
199 double lng;
200 if (QLocationUtils::getNmeaLatLong(latString: parts[3], latDirection: parts[4][0], lngString: parts[5], lngDirection: parts[6][0], lat: &lat, lon: &lng)) {
201 coord.setLatitude(lat);
202 coord.setLongitude(lng);
203 }
204 }
205
206 bool parsed = false;
207 double value = 0.0;
208 if (parts.count() > 7 && parts[7].count() > 0) {
209 value = parts[7].toDouble(ok: &parsed);
210 if (parsed)
211 info->setAttribute(attribute: QGeoPositionInfo::GroundSpeed, value: qreal(value * 1.852 / 3.6)); // knots -> m/s
212 }
213 if (parts.count() > 8 && parts[8].count() > 0) {
214 value = parts[8].toDouble(ok: &parsed);
215 if (parsed)
216 info->setAttribute(attribute: QGeoPositionInfo::Direction, value: qreal(value));
217 }
218 if (parts.count() > 11 && parts[11].count() == 1
219 && (parts[11][0] == 'E' || parts[11][0] == 'W')) {
220 value = parts[10].toDouble(ok: &parsed);
221 if (parsed) {
222 if (parts[11][0] == 'W')
223 value *= -1;
224 info->setAttribute(attribute: QGeoPositionInfo::MagneticVariation, value: qreal(value));
225 }
226 }
227
228 if (coord.type() != QGeoCoordinate::InvalidCoordinate)
229 info->setCoordinate(coord);
230
231 info->setTimestamp(QDateTime(date, time, Qt::UTC));
232}
233
234static void qlocationutils_readVtg(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
235{
236 if (hasFix)
237 *hasFix = false;
238
239 QByteArray sentence(data, size);
240 QList<QByteArray> parts = sentence.split(sep: ',');
241
242 bool parsed = false;
243 double value = 0.0;
244 if (parts.count() > 1 && parts[1].count() > 0) {
245 value = parts[1].toDouble(ok: &parsed);
246 if (parsed)
247 info->setAttribute(attribute: QGeoPositionInfo::Direction, value: qreal(value));
248 }
249 if (parts.count() > 7 && parts[7].count() > 0) {
250 value = parts[7].toDouble(ok: &parsed);
251 if (parsed)
252 info->setAttribute(attribute: QGeoPositionInfo::GroundSpeed, value: qreal(value / 3.6)); // km/h -> m/s
253 }
254}
255
256static void qlocationutils_readZda(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
257{
258 if (hasFix)
259 *hasFix = false;
260
261 QByteArray sentence(data, size);
262 QList<QByteArray> parts = sentence.split(sep: ',');
263 QDate date;
264 QTime time;
265
266 if (parts.count() > 1 && parts[1].count() > 0)
267 QLocationUtils::getNmeaTime(bytes: parts[1], time: &time);
268
269 if (parts.count() > 4 && parts[2].count() > 0 && parts[3].count() > 0
270 && parts[4].count() == 4) { // must be full 4-digit year
271 int day = parts[2].toUInt();
272 int month = parts[3].toUInt();
273 int year = parts[4].toUInt();
274 if (day > 0 && month > 0 && year > 0)
275 date.setDate(year, month, day);
276 }
277
278 info->setTimestamp(QDateTime(date, time, Qt::UTC));
279}
280
281QLocationUtils::NmeaSentence QLocationUtils::getNmeaSentenceType(const char *data, int size)
282{
283 if (size < 6 || data[0] != '$' || !hasValidNmeaChecksum(data, size))
284 return NmeaSentenceInvalid;
285
286 if (data[3] == 'G' && data[4] == 'G' && data[5] == 'A')
287 return NmeaSentenceGGA;
288
289 if (data[3] == 'G' && data[4] == 'S' && data[5] == 'A')
290 return NmeaSentenceGSA;
291
292 if (data[3] == 'G' && data[4] == 'S' && data[5] == 'V')
293 return NmeaSentenceGSV;
294
295 if (data[3] == 'G' && data[4] == 'L' && data[5] == 'L')
296 return NmeaSentenceGLL;
297
298 if (data[3] == 'R' && data[4] == 'M' && data[5] == 'C')
299 return NmeaSentenceRMC;
300
301 if (data[3] == 'V' && data[4] == 'T' && data[5] == 'G')
302 return NmeaSentenceVTG;
303
304 if (data[3] == 'Z' && data[4] == 'D' && data[5] == 'A')
305 return NmeaSentenceZDA;
306
307 return NmeaSentenceInvalid;
308}
309
310bool QLocationUtils::getPosInfoFromNmea(const char *data, int size, QGeoPositionInfo *info,
311 double uere, bool *hasFix)
312{
313 if (!info)
314 return false;
315
316 if (hasFix)
317 *hasFix = false;
318
319 NmeaSentence nmeaType = getNmeaSentenceType(data, size);
320 if (nmeaType == NmeaSentenceInvalid)
321 return false;
322
323 // Adjust size so that * and following characters are not parsed by the following functions.
324 for (int i = 0; i < size; ++i) {
325 if (data[i] == '*') {
326 size = i;
327 break;
328 }
329 }
330
331 switch (nmeaType) {
332 case NmeaSentenceGGA:
333 qlocationutils_readGga(data, size, info, uere, hasFix);
334 return true;
335 case NmeaSentenceGSA:
336 qlocationutils_readGsa(data, size, info, uere, hasFix);
337 return true;
338 case NmeaSentenceGLL:
339 qlocationutils_readGll(data, size, info, hasFix);
340 return true;
341 case NmeaSentenceRMC:
342 qlocationutils_readRmc(data, size, info, hasFix);
343 return true;
344 case NmeaSentenceVTG:
345 qlocationutils_readVtg(data, size, info, hasFix);
346 return true;
347 case NmeaSentenceZDA:
348 qlocationutils_readZda(data, size, info, hasFix);
349 return true;
350 default:
351 return false;
352 }
353}
354
355QLocationUtils::GSVParseStatus QLocationUtils::getSatInfoFromNmea(const char *data, int size, QList<QGeoSatelliteInfo> &infos)
356{
357 if (!data || !size)
358 return GSVNotParsed;
359
360 NmeaSentence nmeaType = getNmeaSentenceType(data, size);
361 if (nmeaType != NmeaSentenceGSV)
362 return GSVNotParsed;
363
364 QList<QByteArray> parts = QByteArray::fromRawData(data, size).split(sep: ',');
365
366 if (parts.count() <= 3) {
367 infos.clear();
368 return GSVFullyParsed; // Malformed sentence.
369 }
370 bool ok;
371 const int totalSentences = parts.at(i: 1).toInt(ok: &ok);
372 if (!ok) {
373 infos.clear();
374 return GSVFullyParsed; // Malformed sentence.
375 }
376
377 const int sentence = parts.at(i: 2).toInt(ok: &ok);
378 if (!ok) {
379 infos.clear();
380 return GSVFullyParsed; // Malformed sentence.
381 }
382
383 const int totalSats = parts.at(i: 3).toInt(ok: &ok);
384 if (!ok) {
385 infos.clear();
386 return GSVFullyParsed; // Malformed sentence.
387 }
388
389 if (sentence == 1)
390 infos.clear();
391
392 const int numSatInSentence = qMin(a: sentence * 4, b: totalSats) - (sentence - 1) * 4;
393
394 int field = 4;
395 for (int i = 0; i < numSatInSentence; ++i) {
396 QGeoSatelliteInfo info;
397 const int prn = parts.at(i: field++).toInt(ok: &ok);
398 info.setSatelliteIdentifier((ok) ? prn : 0);
399 const int elevation = parts.at(i: field++).toInt(ok: &ok);
400 info.setAttribute(attribute: QGeoSatelliteInfo::Elevation, value: (ok) ? elevation : 0);
401 const int azimuth = parts.at(i: field++).toInt(ok: &ok);
402 info.setAttribute(attribute: QGeoSatelliteInfo::Azimuth, value: (ok) ? azimuth : 0);
403 const int snr = parts.at(i: field++).toInt(ok: &ok);
404 info.setSignalStrength((ok) ? snr : -1);
405 infos.append(t: info);
406 }
407
408 if (sentence == totalSentences)
409 return GSVFullyParsed;
410 return GSVPartiallyParsed;
411}
412
413bool QLocationUtils::getSatInUseFromNmea(const char *data, int size, QList<int> &pnrsInUse)
414{
415 pnrsInUse.clear();
416 if (!data || !size)
417 return false;
418
419 NmeaSentence nmeaType = getNmeaSentenceType(data, size);
420 if (nmeaType != NmeaSentenceGSA)
421 return false;
422
423 // Adjust size so that * and following characters are not parsed by the following functions.
424 for (int i = 0; i < size; ++i) {
425 if (data[i] == '*') {
426 size = i;
427 break;
428 }
429 }
430 qlocationutils_readGsa(data, size, pnrsInUse);
431 return true;
432}
433
434bool QLocationUtils::hasValidNmeaChecksum(const char *data, int size)
435{
436 int asteriskIndex = -1;
437 for (int i = 0; i < size; ++i) {
438 if (data[i] == '*') {
439 asteriskIndex = i;
440 break;
441 }
442 }
443
444 const int CSUM_LEN = 2;
445 if (asteriskIndex < 0 || asteriskIndex + CSUM_LEN >= size)
446 return false;
447
448 // XOR byte value of all characters between '$' and '*'
449 int result = 0;
450 for (int i = 1; i < asteriskIndex; ++i)
451 result ^= data[i];
452 /*
453 char calc[CSUM_LEN + 1];
454 ::snprintf(calc, CSUM_LEN + 1, "%02x", result);
455 return ::strncmp(calc, &data[asteriskIndex+1], 2) == 0;
456 */
457
458 QByteArray checkSumBytes(&data[asteriskIndex + 1], 2);
459 bool ok = false;
460 int checksum = checkSumBytes.toInt(ok: &ok,base: 16);
461 return ok && checksum == result;
462}
463
464bool QLocationUtils::getNmeaTime(const QByteArray &bytes, QTime *time)
465{
466 int dotIndex = bytes.indexOf(c: '.');
467 QTime tempTime;
468
469 if (dotIndex < 0) {
470 tempTime = QTime::fromString(s: QString::fromLatin1(str: bytes.constData()),
471 QStringLiteral("hhmmss"));
472 } else {
473 tempTime = QTime::fromString(s: QString::fromLatin1(str: bytes.mid(index: 0, len: dotIndex)),
474 QStringLiteral("hhmmss"));
475 bool hasMsecs = false;
476 int midLen = qMin(a: 3, b: bytes.size() - dotIndex - 1);
477 int msecs = bytes.mid(index: dotIndex + 1, len: midLen).toUInt(ok: &hasMsecs);
478 if (hasMsecs)
479 tempTime = tempTime.addMSecs(ms: msecs*(midLen == 3 ? 1 : midLen == 2 ? 10 : 100));
480 }
481
482 if (tempTime.isValid()) {
483 *time = tempTime;
484 return true;
485 }
486 return false;
487}
488
489bool QLocationUtils::getNmeaLatLong(const QByteArray &latString, char latDirection, const QByteArray &lngString, char lngDirection, double *lat, double *lng)
490{
491 if ((latDirection != 'N' && latDirection != 'S')
492 || (lngDirection != 'E' && lngDirection != 'W')) {
493 return false;
494 }
495
496 bool hasLat = false;
497 bool hasLong = false;
498 double tempLat = latString.toDouble(ok: &hasLat);
499 double tempLng = lngString.toDouble(ok: &hasLong);
500 if (hasLat && hasLong) {
501 tempLat = qlocationutils_nmeaDegreesToDecimal(nmeaDegrees: tempLat);
502 if (latDirection == 'S')
503 tempLat *= -1;
504 tempLng = qlocationutils_nmeaDegreesToDecimal(nmeaDegrees: tempLng);
505 if (lngDirection == 'W')
506 tempLng *= -1;
507
508 if (isValidLat(lat: tempLat) && isValidLong(lng: tempLng)) {
509 *lat = tempLat;
510 *lng = tempLng;
511 return true;
512 }
513 }
514 return false;
515}
516
517QT_END_NAMESPACE
518
519

source code of qtlocation/src/positioning/qlocationutils.cpp