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 // Multiple: GN
298 if (key.startsWith("GN"))
299 return QGeoSatelliteInfo::Multiple;
300
301 return QGeoSatelliteInfo::Undefined;
302}
303
304QGeoSatelliteInfo::SatelliteSystem QLocationUtils::getSatelliteSystemBySatelliteId(int satId)
305{
306 if (satId >= 1 && satId <= 32)
307 return QGeoSatelliteInfo::GPS;
308
309 if (satId >= 65 && satId <= 96) // including future extensions
310 return QGeoSatelliteInfo::GLONASS;
311
312 if (satId >= 193 && satId <= 200) // including future extensions
313 return QGeoSatelliteInfo::QZSS;
314
315 if ((satId >= 201 && satId <= 235) || (satId >= 401 && satId <= 437))
316 return QGeoSatelliteInfo::BEIDOU;
317
318 if (satId >= 301 && satId <= 336)
319 return QGeoSatelliteInfo::GALILEO;
320
321 return QGeoSatelliteInfo::Undefined;
322}
323
324bool QLocationUtils::getPosInfoFromNmea(QByteArrayView bv, QGeoPositionInfo *info,
325 double uere, bool *hasFix)
326{
327 if (!info)
328 return false;
329
330 if (hasFix)
331 *hasFix = false;
332
333 NmeaSentence nmeaType = getNmeaSentenceType(bv);
334 if (nmeaType == NmeaSentenceInvalid)
335 return false;
336
337 // Adjust size so that * and following characters are not parsed by the following functions.
338 qsizetype idx = bv.indexOf('*');
339 QByteArrayView key = idx < 0 ? bv : bv.first(idx);
340
341 switch (nmeaType) {
342 case NmeaSentenceGGA:
343 qlocationutils_readGga(key, info, uere, hasFix);
344 return true;
345 case NmeaSentenceGSA:
346 qlocationutils_readGsa(key, info, uere, hasFix);
347 return true;
348 case NmeaSentenceGLL:
349 qlocationutils_readGll(key, info, hasFix);
350 return true;
351 case NmeaSentenceRMC:
352 qlocationutils_readRmc(key, info, hasFix);
353 return true;
354 case NmeaSentenceVTG:
355 qlocationutils_readVtg(key, info, hasFix);
356 return true;
357 case NmeaSentenceZDA:
358 qlocationutils_readZda(key, info, hasFix);
359 return true;
360 default:
361 return false;
362 }
363}
364
365QNmeaSatelliteInfoSource::SatelliteInfoParseStatus
366QLocationUtils::getSatInfoFromNmea(QByteArrayView bv, QList<QGeoSatelliteInfo> &infos, QGeoSatelliteInfo::SatelliteSystem &system)
367{
368 if (bv.isEmpty())
369 return QNmeaSatelliteInfoSource::NotParsed;
370
371 NmeaSentence nmeaType = getNmeaSentenceType(bv);
372 if (nmeaType != NmeaSentenceGSV)
373 return QNmeaSatelliteInfoSource::NotParsed;
374
375 // Standard forbids using $GN talker id for GSV messages, so the system
376 // type here will be uniquely identified.
377 system = getSatelliteSystem(bv);
378
379 // Adjust size so that * and following characters are not parsed by the
380 // following code.
381 qsizetype idx = bv.indexOf('*');
382
383 const QList<QByteArray> parts = QByteArray::fromRawData(bv.data(),
384 idx < 0 ? bv.size() : idx).split(',');
385
386 if (parts.size() <= 3) {
387 infos.clear();
388 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
389 }
390 bool ok;
391 const int totalSentences = parts.at(1).toInt(&ok);
392 if (!ok) {
393 infos.clear();
394 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
395 }
396
397 const int sentence = parts.at(2).toInt(&ok);
398 if (!ok) {
399 infos.clear();
400 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
401 }
402
403 const int totalSats = parts.at(3).toInt(&ok);
404 if (!ok) {
405 infos.clear();
406 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
407 }
408
409 if (sentence == 1)
410 infos.clear();
411
412 const int numSatInSentence = qMin(sentence * 4, totalSats) - (sentence - 1) * 4;
413 if (parts.size() < (4 + numSatInSentence * 4)) {
414 infos.clear();
415 return QNmeaSatelliteInfoSource::FullyParsed; // Malformed sentence.
416 }
417
418 int field = 4;
419 for (int i = 0; i < numSatInSentence; ++i) {
420 QGeoSatelliteInfo info;
421 info.setSatelliteSystem(system);
422 int prn = parts.at(field++).toInt(&ok);
423 // Quote from: https://gpsd.gitlab.io/gpsd/NMEA.html#_satellite_ids
424 // GLONASS satellite numbers come in two flavors. If a sentence has a GL
425 // talker ID, expect the skyviews to be GLONASS-only and in the range
426 // 1-32; you must add 64 to get a globally-unique NMEA ID. If the
427 // sentence has a GN talker ID, the device emits a multi-constellation
428 // skyview with GLONASS IDs already in the 65-96 range.
429 //
430 // However I don't observe such behavior with my device. So implementing
431 // a safe scenario.
432 if (ok && (system == QGeoSatelliteInfo::GLONASS)) {
433 if (prn <= 64)
434 prn += 64;
435 }
436 info.setSatelliteIdentifier((ok) ? prn : 0);
437 const int elevation = parts.at(field++).toInt(&ok);
438 info.setAttribute(QGeoSatelliteInfo::Elevation, (ok) ? elevation : 0);
439 const int azimuth = parts.at(field++).toInt(&ok);
440 info.setAttribute(QGeoSatelliteInfo::Azimuth, (ok) ? azimuth : 0);
441 const int snr = parts.at(field++).toInt(&ok);
442 info.setSignalStrength((ok) ? snr : -1);
443 infos.append(info);
444 }
445
446 if (sentence == totalSentences)
447 return QNmeaSatelliteInfoSource::FullyParsed;
448
449 return QNmeaSatelliteInfoSource::PartiallyParsed;
450}
451
452QGeoSatelliteInfo::SatelliteSystem QLocationUtils::getSatInUseFromNmea(QByteArrayView bv,
453 QList<int> &pnrsInUse)
454{
455 if (bv.isEmpty())
456 return QGeoSatelliteInfo::Undefined;
457
458 NmeaSentence nmeaType = getNmeaSentenceType(bv);
459 if (nmeaType != NmeaSentenceGSA)
460 return QGeoSatelliteInfo::Undefined;
461
462 auto systemType = getSatelliteSystem(bv);
463 if (systemType == QGeoSatelliteInfo::Undefined)
464 return systemType;
465
466 // The documentation states that we do not modify pnrsInUse if we could not
467 // parse the data
468 pnrsInUse.clear();
469
470 // Adjust size so that * and following characters are not parsed by the following functions.
471 qsizetype idx = bv.indexOf('*');
472 QByteArrayView key = idx < 0 ? bv : bv.first(idx);
473
474 qlocationutils_readGsa(key, pnrsInUse);
475
476 // Quote from: https://gpsd.gitlab.io/gpsd/NMEA.html#_satellite_ids
477 // GLONASS satellite numbers come in two flavors. If a sentence has a GL
478 // talker ID, expect the skyviews to be GLONASS-only and in the range 1-32;
479 // you must add 64 to get a globally-unique NMEA ID. If the sentence has a
480 // GN talker ID, the device emits a multi-constellation skyview with
481 // GLONASS IDs already in the 65-96 range.
482 //
483 // However I don't observe such behavior with my device. So implementing a
484 // safe scenario.
485 if (systemType == QGeoSatelliteInfo::GLONASS) {
486 std::for_each(pnrsInUse.begin(), pnrsInUse.end(), [](int &id) {
487 if (id <= 64)
488 id += 64;
489 });
490 }
491
492 if ((systemType == QGeoSatelliteInfo::Multiple) && !pnrsInUse.isEmpty()) {
493 // Standard claims that in case of multiple system types we will receive
494 // several GSA messages, each containing data from only one satellite
495 // system, so we can pick the first id to determine the system type.
496 auto tempSystemType = getSatelliteSystemBySatelliteId(pnrsInUse.front());
497 if (tempSystemType != QGeoSatelliteInfo::Undefined)
498 systemType = tempSystemType;
499 }
500
501 return systemType;
502}
503
504bool QLocationUtils::hasValidNmeaChecksum(QByteArrayView bv)
505{
506 qsizetype asteriskIndex = bv.indexOf('*');
507
508 constexpr qsizetype CSUM_LEN = 2;
509 if (asteriskIndex < 0 || asteriskIndex >= bv.size() - CSUM_LEN)
510 return false;
511
512 // XOR byte value of all characters between '$' and '*'
513 int result = 0;
514 for (qsizetype i = 1; i < asteriskIndex; ++i)
515 result ^= bv[i];
516 /*
517 char calc[CSUM_LEN + 1];
518 ::snprintf(calc, CSUM_LEN + 1, "%02x", result);
519 return ::strncmp(calc, &data[asteriskIndex+1], 2) == 0;
520 */
521
522 QByteArrayView checkSumBytes = bv.sliced(asteriskIndex + 1, 2);
523 bool ok = false;
524 int checksum = checkSumBytes.toInt(&ok,16);
525 return ok && checksum == result;
526}
527
528bool QLocationUtils::getNmeaTime(const QByteArray &bytes, QTime *time)
529{
530 QTime tempTime = QTime::fromString(QString::fromLatin1(bytes),
531 QStringView(bytes.size() > 6 && bytes[6] == '.'
532 ? u"hhmmss.z"
533 : u"hhmmss"));
534
535 if (tempTime.isValid()) {
536 *time = tempTime;
537 return true;
538 }
539 return false;
540}
541
542bool QLocationUtils::getNmeaLatLong(const QByteArray &latString, char latDirection, const QByteArray &lngString, char lngDirection, double *lat, double *lng)
543{
544 if ((latDirection != 'N' && latDirection != 'S')
545 || (lngDirection != 'E' && lngDirection != 'W')) {
546 return false;
547 }
548
549 bool hasLat = false;
550 bool hasLong = false;
551 double tempLat = latString.toDouble(&hasLat);
552 double tempLng = lngString.toDouble(&hasLong);
553 if (hasLat && hasLong) {
554 tempLat = qlocationutils_nmeaDegreesToDecimal(tempLat);
555 if (latDirection == 'S')
556 tempLat *= -1;
557 tempLng = qlocationutils_nmeaDegreesToDecimal(tempLng);
558 if (lngDirection == 'W')
559 tempLng *= -1;
560
561 if (isValidLat(tempLat) && isValidLong(tempLng)) {
562 *lat = tempLat;
563 *lng = tempLng;
564 return true;
565 }
566 }
567 return false;
568}
569
570QT_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