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