Qt
Internal/Contributor docs for the Qt SDK. Note: These are NOT official API docs; those are found at https://doc.qt.io/
Loading...
Searching...
No Matches
qlocationutils.cpp
Go to the documentation of this file.
1// Copyright (C) 2016 Jolla Ltd.
2// Copyright (C) 2016 The Qt Company Ltd.
3// SPDX-License-Identifier: LicenseRef-Qt-Commercial OR LGPL-3.0-only OR GPL-2.0-only OR GPL-3.0-only
4// Qt-Security score:critical reason:data-parser
5
6#include "qlocationutils_p.h"
7#include "qgeopositioninfo.h"
8#include "qgeosatelliteinfo.h"
9
10#include <QTime>
11#include <QList>
12#include <QByteArray>
13#include <QDateTime>
14#include <QDebug>
15#include <QTimeZone>
16
17#include <math.h>
18
19QT_BEGIN_NAMESPACE
20
21// converts e.g. 15306.0235 from NMEA sentence to 153.100392
22static double qlocationutils_nmeaDegreesToDecimal(double nmeaDegrees)
23{
24 double deg;
25 double min = 100.0 * modf(nmeaDegrees / 100.0, &deg);
26 return deg + (min / 60.0);
27}
28
29static void qlocationutils_readGga(QByteArrayView bv, QGeoPositionInfo *info, double uere,
30 bool *hasFix)
31{
32 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(), bv.size()).split(',');
33 QGeoCoordinate coord;
34
35 if (hasFix && parts.size() > 6 && !parts[6].isEmpty())
36 *hasFix = parts[6].toInt() > 0;
37
38 if (parts.size() > 1 && !parts[1].isEmpty()) {
39 QTime time;
40 if (QLocationUtils::getNmeaTime(parts[1], &time))
41 info->setTimestamp(QDateTime(QDate(), time, QTimeZone::UTC));
42 }
43
44 if (parts.size() > 5 && parts[3].size() == 1 && parts[5].size() == 1) {
45 double lat;
46 double lng;
47 if (QLocationUtils::getNmeaLatLong(parts[2], parts[3][0], parts[4], parts[5][0], &lat, &lng)) {
48 coord.setLatitude(lat);
49 coord.setLongitude(lng);
50 }
51 }
52
53 if (parts.size() > 8 && !parts[8].isEmpty()) {
54 bool hasHdop = false;
55 double hdop = parts[8].toDouble(&hasHdop);
56 if (hasHdop)
57 info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere);
58 }
59
60 if (parts.size() > 9 && !parts[9].isEmpty()) {
61 bool hasAlt = false;
62 double alt = parts[9].toDouble(&hasAlt);
63 if (hasAlt)
64 coord.setAltitude(alt);
65 }
66
67 if (coord.type() != QGeoCoordinate::InvalidCoordinate)
68 info->setCoordinate(coord);
69}
70
71static void qlocationutils_readGsa(QByteArrayView bv, QGeoPositionInfo *info, double uere,
72 bool *hasFix)
73{
74 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(), bv.size()).split(',');
75
76 if (hasFix && parts.size() > 2 && !parts[2].isEmpty())
77 *hasFix = parts[2].toInt() > 0;
78
79 if (parts.size() > 16 && !parts[16].isEmpty()) {
80 bool hasHdop = false;
81 double hdop = parts[16].toDouble(&hasHdop);
82 if (hasHdop)
83 info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere);
84 }
85
86 if (parts.size() > 17 && !parts[17].isEmpty()) {
87 bool hasVdop = false;
88 double vdop = parts[17].toDouble(&hasVdop);
89 if (hasVdop)
90 info->setAttribute(QGeoPositionInfo::VerticalAccuracy, 2 * vdop * uere);
91 }
92}
93
94static void qlocationutils_readGsa(QByteArrayView bv, QList<int> &pnrsInUse)
95{
96 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(), bv.size()).split(',');
97 pnrsInUse.clear();
98 if (parts.size() <= 2)
99 return;
100 bool ok;
101 for (qsizetype i = 3; i < qMin(15, parts.size()); ++i) {
102 const QByteArray &pnrString = parts.at(i);
103 if (pnrString.isEmpty())
104 continue;
105 int pnr = pnrString.toInt(&ok);
106 if (ok)
107 pnrsInUse.append(pnr);
108 }
109}
110
111static void qlocationutils_readGll(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
112{
113 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(), bv.size()).split(',');
114 QGeoCoordinate coord;
115
116 if (hasFix && parts.size() > 6 && !parts[6].isEmpty())
117 *hasFix = (parts[6][0] == 'A');
118
119 if (parts.size() > 5 && !parts[5].isEmpty()) {
120 QTime time;
121 if (QLocationUtils::getNmeaTime(parts[5], &time))
122 info->setTimestamp(QDateTime(QDate(), time, QTimeZone::UTC));
123 }
124
125 if (parts.size() > 4 && parts[2].size() == 1 && parts[4].size() == 1) {
126 double lat;
127 double lng;
128 if (QLocationUtils::getNmeaLatLong(parts[1], parts[2][0], parts[3], parts[4][0], &lat, &lng)) {
129 coord.setLatitude(lat);
130 coord.setLongitude(lng);
131 }
132 }
133
134 if (coord.type() != QGeoCoordinate::InvalidCoordinate)
135 info->setCoordinate(coord);
136}
137
138static void qlocationutils_readRmc(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
139{
140 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(), bv.size()).split(',');
141 QGeoCoordinate coord;
142 QDate date;
143 QTime time;
144
145 if (hasFix && parts.size() > 2 && !parts[2].isEmpty())
146 *hasFix = (parts[2][0] == 'A');
147
148 if (parts.size() > 9 && parts[9].size() == 6) {
149 date = QDate::fromString(QString::fromLatin1(parts[9]), QStringLiteral("ddMMyy"));
150 if (date.isValid())
151 date = date.addYears(100); // otherwise starts from 1900
152 }
153
154 if (parts.size() > 1 && !parts[1].isEmpty())
155 QLocationUtils::getNmeaTime(parts[1], &time);
156
157 if (parts.size() > 6 && parts[4].size() == 1 && parts[6].size() == 1) {
158 double lat;
159 double lng;
160 if (QLocationUtils::getNmeaLatLong(parts[3], parts[4][0], parts[5], parts[6][0], &lat, &lng)) {
161 coord.setLatitude(lat);
162 coord.setLongitude(lng);
163 }
164 }
165
166 bool parsed = false;
167 double value = 0.0;
168 if (parts.size() > 7 && !parts[7].isEmpty()) {
169 value = parts[7].toDouble(&parsed);
170 if (parsed)
171 info->setAttribute(QGeoPositionInfo::GroundSpeed, qreal(value * 1.852 / 3.6)); // knots -> m/s
172 }
173 if (parts.size() > 8 && !parts[8].isEmpty()) {
174 value = parts[8].toDouble(&parsed);
175 if (parsed)
176 info->setAttribute(QGeoPositionInfo::Direction, qreal(value));
177 }
178 if (parts.size() > 11 && parts[11].size() == 1
179 && (parts[11][0] == 'E' || parts[11][0] == 'W')) {
180 value = parts[10].toDouble(&parsed);
181 if (parsed) {
182 if (parts[11][0] == 'W')
183 value *= -1;
184 info->setAttribute(QGeoPositionInfo::MagneticVariation, qreal(value));
185 }
186 }
187
188 if (coord.type() != QGeoCoordinate::InvalidCoordinate)
189 info->setCoordinate(coord);
190
191 info->setTimestamp(QDateTime(date, time, QTimeZone::UTC));
192}
193
194static void qlocationutils_readVtg(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
195{
196 if (hasFix)
197 *hasFix = false;
198
199 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(), bv.size()).split(',');
200
201 bool parsed = false;
202 double value = 0.0;
203 if (parts.size() > 1 && !parts[1].isEmpty()) {
204 value = parts[1].toDouble(&parsed);
205 if (parsed)
206 info->setAttribute(QGeoPositionInfo::Direction, qreal(value));
207 }
208 if (parts.size() > 7 && !parts[7].isEmpty()) {
209 value = parts[7].toDouble(&parsed);
210 if (parsed)
211 info->setAttribute(QGeoPositionInfo::GroundSpeed, qreal(value / 3.6)); // km/h -> m/s
212 }
213}
214
215static void qlocationutils_readZda(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
216{
217 if (hasFix)
218 *hasFix = false;
219
220 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(), bv.size()).split(',');
221 QDate date;
222 QTime time;
223
224 if (parts.size() > 1 && !parts[1].isEmpty())
225 QLocationUtils::getNmeaTime(parts[1], &time);
226
227 if (parts.size() > 4 && !parts[2].isEmpty() && !parts[3].isEmpty()
228 && parts[4].size() == 4) { // must be full 4-digit year
229 int day = parts[2].toInt();
230 int month = parts[3].toInt();
231 int year = parts[4].toInt();
232 if (day > 0 && month > 0 && year > 0)
233 date.setDate(year, month, day);
234 }
235
236 info->setTimestamp(QDateTime(date, time, QTimeZone::UTC));
237}
238
239QLocationUtils::NmeaSentence QLocationUtils::getNmeaSentenceType(QByteArrayView bv)
240{
241 if (bv.size() < 6 || bv[0] != '$' || !hasValidNmeaChecksum(bv))
242 return NmeaSentenceInvalid;
243
244 QByteArrayView key = bv.sliced(3);
245
246 if (key.startsWith("GGA"))
247 return NmeaSentenceGGA;
248
249 if (key.startsWith("GSA"))
250 return NmeaSentenceGSA;
251
252 if (key.startsWith("GSV"))
253 return NmeaSentenceGSV;
254
255 if (key.startsWith("GLL"))
256 return NmeaSentenceGLL;
257
258 if (key.startsWith("RMC"))
259 return NmeaSentenceRMC;
260
261 if (key.startsWith("VTG"))
262 return NmeaSentenceVTG;
263
264 if (key.startsWith("ZDA"))
265 return NmeaSentenceZDA;
266
267 return NmeaSentenceInvalid;
268}
269
270QGeoSatelliteInfo::SatelliteSystem QLocationUtils::getSatelliteSystem(QByteArrayView bv)
271{
272 if (bv.size() < 6 || bv[0] != '$' || !hasValidNmeaChecksum(bv))
273 return QGeoSatelliteInfo::Undefined;
274
275 QByteArrayView key = bv.sliced(1);
276
277 // GPS: GP
278 if (key.startsWith("GP"))
279 return QGeoSatelliteInfo::GPS;
280
281 // GLONASS: GL
282 if (key.startsWith("GL"))
283 return QGeoSatelliteInfo::GLONASS;
284
285 // GALILEO: GA
286 if (key.startsWith("GA"))
287 return QGeoSatelliteInfo::GALILEO;
288
289 // BeiDou: BD or GB
290 if (key.startsWith("BD") || key.startsWith("GB"))
291 return QGeoSatelliteInfo::BEIDOU;
292
293 // QZSS: GQ, PQ, QZ
294 if (key.startsWith("GQ") || key.startsWith("PQ") || key.startsWith("QZ"))
295 return QGeoSatelliteInfo::QZSS;
296
297 // IRNSS: GI
298 if (key.startsWith("GI"))
299 return QGeoSatelliteInfo::IRNSS;
300
301 // SBAS: no talker id, should use satellite id range
302
303 // Multiple: GN
304 if (key.startsWith("GN"))
305 return QGeoSatelliteInfo::Multiple;
306
307 return QGeoSatelliteInfo::Undefined;
308}
309
310QGeoSatelliteInfo::SatelliteSystem QLocationUtils::getSatelliteSystemBySatelliteId(int satId)
311{
312 if (satId >= 1 && satId <= 32)
313 return QGeoSatelliteInfo::GPS;
314
315 if (satId >= 33 && satId <= 64)
316 return QGeoSatelliteInfo::SBAS;
317
318 if (satId >= 65 && satId <= 96) // including future extensions
319 return QGeoSatelliteInfo::GLONASS;
320
321 if (satId >= 193 && satId <= 200) // including future extensions
322 return QGeoSatelliteInfo::QZSS;
323
324 if ((satId >= 201 && satId <= 235) || (satId >= 401 && satId <= 437))
325 return QGeoSatelliteInfo::BEIDOU;
326
327 if (satId >= 301 && satId <= 336)
328 return QGeoSatelliteInfo::GALILEO;
329
330 return QGeoSatelliteInfo::Undefined;
331}
332
333bool QLocationUtils::getPosInfoFromNmea(QByteArrayView bv, QGeoPositionInfo *info,
334 double uere, bool *hasFix)
335{
336 if (!info)
337 return false;
338
339 if (hasFix)
340 *hasFix = false;
341
342 NmeaSentence nmeaType = getNmeaSentenceType(bv);
343 if (nmeaType == NmeaSentenceInvalid)
344 return false;
345
346 // Adjust size so that * and following characters are not parsed by the following functions.
347 qsizetype idx = bv.indexOf('*');
348 QByteArrayView key = idx < 0 ? bv : bv.first(idx);
349
350 switch (nmeaType) {
351 case NmeaSentenceGGA:
352 qlocationutils_readGga(key, info, uere, hasFix);
353 return true;
354 case NmeaSentenceGSA:
355 qlocationutils_readGsa(key, info, uere, hasFix);
356 return true;
357 case NmeaSentenceGLL:
358 qlocationutils_readGll(key, info, hasFix);
359 return true;
360 case NmeaSentenceRMC:
361 qlocationutils_readRmc(key, info, hasFix);
362 return true;
363 case NmeaSentenceVTG:
364 qlocationutils_readVtg(key, info, hasFix);
365 return true;
366 case NmeaSentenceZDA:
367 qlocationutils_readZda(key, info, hasFix);
368 return true;
369 default:
370 return false;
371 }
372}
373
374QNmeaSatelliteInfoSource::SatelliteInfoParseStatus
375QLocationUtils::getSatInfoFromNmea(QByteArrayView bv, QList<QGeoSatelliteInfo> &infos, QGeoSatelliteInfo::SatelliteSystem &system)
376{
377 if (bv.isEmpty())
378 return QNmeaSatelliteInfoSource::NotParsed;
379
380 NmeaSentence nmeaType = getNmeaSentenceType(bv);
381 if (nmeaType != NmeaSentenceGSV)
382 return QNmeaSatelliteInfoSource::NotParsed;
383
384 // Standard forbids using $GN talker id for GSV messages, so the system
385 // type here will be uniquely identified.
386 system = getSatelliteSystem(bv);
387
388 // Adjust size so that * and following characters are not parsed by the
389 // following code.
390 qsizetype idx = bv.indexOf('*');
391
392 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(),
393 idx < 0 ? bv.size() : idx).split(',');
394
395 if (parts.size() <= 3) {
396 infos.clear();
397 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
398 }
399 bool ok;
400 const int totalSentences = parts.at(1).toInt(&ok);
401 if (!ok) {
402 infos.clear();
403 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
404 }
405
406 const int sentence = parts.at(2).toInt(&ok);
407 if (!ok) {
408 infos.clear();
409 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
410 }
411
412 const int totalSats = parts.at(3).toInt(&ok);
413 if (!ok) {
414 infos.clear();
415 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
416 }
417
418 if (sentence == 1)
419 infos.clear();
420
421 const int numSatInSentence = qMin(sentence * 4, totalSats) - (sentence - 1) * 4;
422 if (parts.size() < (4 + numSatInSentence * 4)) {
423 infos.clear();
424 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
425 }
426
427 int field = 4;
428 for (int i = 0; i < numSatInSentence; ++i) {
429 QGeoSatelliteInfo info;
430 info.setSatelliteSystem(system);
431 int prn = parts.at(field++).toInt(&ok);
432 // Quote from: https://gpsd.gitlab.io/gpsd/NMEA.html#_satellite_ids
433 // GLONASS satellite numbers come in two flavors. If a sentence has a GL
434 // talker ID, expect the skyviews to be GLONASS-only and in the range
435 // 1-32; you must add 64 to get a globally-unique NMEA ID. If the
436 // sentence has a GN talker ID, the device emits a multi-constellation
437 // skyview with GLONASS IDs already in the 65-96 range.
438 //
439 // However I don't observe such behavior with my device. So implementing
440 // a safe scenario.
441 if (ok && (system == QGeoSatelliteInfo::GLONASS)) {
442 if (prn <= 64)
443 prn += 64;
444 }
445
446 // If the system is unknown, try to determine it based on prn.
447 // Note that the new value can vary among the satellites, so *do not*
448 // update the out-variable 'system'.
449 if (ok && system == QGeoSatelliteInfo::Undefined)
450 info.setSatelliteSystem(getSatelliteSystemBySatelliteId(prn));
451
452 info.setSatelliteIdentifier((ok) ? prn : 0);
453 const int elevation = parts.at(field++).toInt(&ok);
454 info.setAttribute(QGeoSatelliteInfo::Elevation, (ok) ? elevation : 0);
455 const int azimuth = parts.at(field++).toInt(&ok);
456 info.setAttribute(QGeoSatelliteInfo::Azimuth, (ok) ? azimuth : 0);
457 const int snr = parts.at(field++).toInt(&ok);
458 info.setSignalStrength((ok) ? snr : -1);
459 infos.append(info);
460 }
461
462 if (sentence == totalSentences)
463 return QNmeaSatelliteInfoSource::FullyParsed;
464
465 return QNmeaSatelliteInfoSource::PartiallyParsed;
466}
467
468QGeoSatelliteInfo::SatelliteSystem QLocationUtils::getSatInUseFromNmea(QByteArrayView bv,
469 QList<int> &pnrsInUse)
470{
471 if (bv.isEmpty())
472 return QGeoSatelliteInfo::Undefined;
473
474 NmeaSentence nmeaType = getNmeaSentenceType(bv);
475 if (nmeaType != NmeaSentenceGSA)
476 return QGeoSatelliteInfo::Undefined;
477
478 auto systemType = getSatelliteSystem(bv);
479 if (systemType == QGeoSatelliteInfo::Undefined)
480 return systemType;
481
482 // The documentation states that we do not modify pnrsInUse if we could not
483 // parse the data
484 pnrsInUse.clear();
485
486 // Adjust size so that * and following characters are not parsed by the following functions.
487 qsizetype idx = bv.indexOf('*');
488 QByteArrayView key = idx < 0 ? bv : bv.first(idx);
489
490 qlocationutils_readGsa(key, pnrsInUse);
491
492 // Quote from: https://gpsd.gitlab.io/gpsd/NMEA.html#_satellite_ids
493 // GLONASS satellite numbers come in two flavors. If a sentence has a GL
494 // talker ID, expect the skyviews to be GLONASS-only and in the range 1-32;
495 // you must add 64 to get a globally-unique NMEA ID. If the sentence has a
496 // GN talker ID, the device emits a multi-constellation skyview with
497 // GLONASS IDs already in the 65-96 range.
498 //
499 // However I don't observe such behavior with my device. So implementing a
500 // safe scenario.
501 if (systemType == QGeoSatelliteInfo::GLONASS) {
502 std::for_each(pnrsInUse.begin(), pnrsInUse.end(), [](int &id) {
503 if (id <= 64)
504 id += 64;
505 });
506 }
507
508 if ((systemType == QGeoSatelliteInfo::Multiple) && !pnrsInUse.isEmpty()) {
509 // Standard claims that in case of multiple system types we will receive
510 // several GSA messages, each containing data from only one satellite
511 // system, so we can pick the first id to determine the system type.
512 auto tempSystemType = getSatelliteSystemBySatelliteId(pnrsInUse.front());
513 if (tempSystemType != QGeoSatelliteInfo::Undefined)
514 systemType = tempSystemType;
515 }
516
517 return systemType;
518}
519
520bool QLocationUtils::hasValidNmeaChecksum(QByteArrayView bv)
521{
522 qsizetype asteriskIndex = bv.indexOf('*');
523
524 constexpr qsizetype CSUM_LEN = 2;
525 if (asteriskIndex < 0 || asteriskIndex >= bv.size() - CSUM_LEN)
526 return false;
527
528 // XOR byte value of all characters between '$' and '*'
529 int result = 0;
530 for (qsizetype i = 1; i < asteriskIndex; ++i)
531 result ^= bv[i];
532 /*
533 char calc[CSUM_LEN + 1];
534 ::snprintf(calc, CSUM_LEN + 1, "%02x", result);
535 return ::strncmp(calc, &data[asteriskIndex+1], 2) == 0;
536 */
537
538 QByteArrayView checkSumBytes = bv.sliced(asteriskIndex + 1, 2);
539 bool ok = false;
540 int checksum = checkSumBytes.toInt(&ok,16);
541 return ok && checksum == result;
542}
543
544bool QLocationUtils::getNmeaTime(const QByteArray &bytes, QTime *time)
545{
546 QTime tempTime = QTime::fromString(QString::fromLatin1(bytes),
547 QStringView(bytes.size() > 6 && bytes[6] == '.'
548 ? u"hhmmss.z"
549 : u"hhmmss"));
550
551 if (tempTime.isValid()) {
552 *time = tempTime;
553 return true;
554 }
555 return false;
556}
557
558bool QLocationUtils::getNmeaLatLong(const QByteArray &latString, char latDirection, const QByteArray &lngString, char lngDirection, double *lat, double *lng)
559{
560 if ((latDirection != 'N' && latDirection != 'S')
561 || (lngDirection != 'E' && lngDirection != 'W')) {
562 return false;
563 }
564
565 bool hasLat = false;
566 bool hasLong = false;
567 double tempLat = latString.toDouble(&hasLat);
568 double tempLng = lngString.toDouble(&hasLong);
569 if (hasLat && hasLong) {
570 tempLat = qlocationutils_nmeaDegreesToDecimal(tempLat);
571 if (latDirection == 'S')
572 tempLat *= -1;
573 tempLng = qlocationutils_nmeaDegreesToDecimal(tempLng);
574 if (lngDirection == 'W')
575 tempLng *= -1;
576
577 if (isValidLat(tempLat) && isValidLong(tempLng)) {
578 *lat = tempLat;
579 *lng = tempLng;
580 return true;
581 }
582 }
583 return false;
584}
585
586QT_END_NAMESPACE
qlocationutils_readZda
static void qlocationutils_readZda(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
Definition qlocationutils.cpp:215
qlocationutils_readVtg
static void qlocationutils_readVtg(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
Definition qlocationutils.cpp:194
qlocationutils_readRmc
static void qlocationutils_readRmc(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
Definition qlocationutils.cpp:138
qlocationutils_readGll
static void qlocationutils_readGll(QByteArrayView bv, QGeoPositionInfo *info, bool *hasFix)
Definition qlocationutils.cpp:111
qlocationutils_readGsa
static void qlocationutils_readGsa(QByteArrayView bv, QList< int > &pnrsInUse)
Definition qlocationutils.cpp:94
qlocationutils_readGga
static void qlocationutils_readGga(QByteArrayView bv, QGeoPositionInfo *info, double uere, bool *hasFix)
Definition qlocationutils.cpp:29
qlocationutils_readGsa
static void qlocationutils_readGsa(QByteArrayView bv, QGeoPositionInfo *info, double uere, bool *hasFix)
Definition qlocationutils.cpp:71