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 
52 QT_BEGIN_NAMESPACE
53 
54 // converts e.g. 15306.0235 from NMEA sentence to 153.100392
qlocationutils_nmeaDegreesToDecimal(double nmeaDegrees)55 static double qlocationutils_nmeaDegreesToDecimal(double nmeaDegrees)
56 {
57     double deg;
58     double min = 100.0 * modf(nmeaDegrees / 100.0, &deg);
59     return deg + (min / 60.0);
60 }
61 
qlocationutils_readGga(const char * data,int size,QGeoPositionInfo * info,double uere,bool * hasFix)62 static 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(',');
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(parts[1], &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(parts[2], parts[3][0], parts[4], parts[5][0], &lat, &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(&hasHdop);
90         if (hasHdop)
91             info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere);
92     }
93 
94     if (parts.count() > 9 && parts[9].count() > 0) {
95         bool hasAlt = false;
96         double alt = parts[9].toDouble(&hasAlt);
97         if (hasAlt)
98             coord.setAltitude(alt);
99     }
100 
101     if (coord.type() != QGeoCoordinate::InvalidCoordinate)
102         info->setCoordinate(coord);
103 }
104 
qlocationutils_readGsa(const char * data,int size,QGeoPositionInfo * info,double uere,bool * hasFix)105 static 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(',');
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(&hasHdop);
116         if (hasHdop)
117             info->setAttribute(QGeoPositionInfo::HorizontalAccuracy, 2 * hdop * uere);
118     }
119 
120     if (parts.count() > 17 && !parts[17].isEmpty()) {
121         bool hasVdop = false;
122         double vdop = parts[17].toDouble(&hasVdop);
123         if (hasVdop)
124             info->setAttribute(QGeoPositionInfo::VerticalAccuracy, 2 * vdop * uere);
125     }
126 }
127 
qlocationutils_readGsa(const char * data,int size,QList<int> & pnrsInUse)128 static void qlocationutils_readGsa(const char *data,
129                                               int size,
130                                               QList<int> &pnrsInUse)
131 {
132     QList<QByteArray> parts = QByteArray::fromRawData(data, size).split(',');
133     pnrsInUse.clear();
134     if (parts.count() <= 2)
135         return;
136     bool ok;
137     for (int i = 3; i <= qMin(14, parts.size()); ++i) {
138         const QByteArray &pnrString = parts.at(i);
139         if (pnrString.isEmpty())
140             continue;
141         int pnr = pnrString.toInt(&ok);
142         if (ok)
143             pnrsInUse.append(pnr);
144     }
145 }
146 
qlocationutils_readGll(const char * data,int size,QGeoPositionInfo * info,bool * hasFix)147 static void qlocationutils_readGll(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
148 {
149     QByteArray sentence(data, size);
150     QList<QByteArray> parts = sentence.split(',');
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(parts[5], &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(parts[1], parts[2][0], parts[3], parts[4][0], &lat, &lng)) {
166             coord.setLatitude(lat);
167             coord.setLongitude(lng);
168         }
169     }
170 
171     if (coord.type() != QGeoCoordinate::InvalidCoordinate)
172         info->setCoordinate(coord);
173 }
174 
qlocationutils_readRmc(const char * data,int size,QGeoPositionInfo * info,bool * hasFix)175 static void qlocationutils_readRmc(const char *data, int size, QGeoPositionInfo *info, bool *hasFix)
176 {
177     QByteArray sentence(data, size);
178     QList<QByteArray> parts = sentence.split(',');
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(QString::fromLatin1(parts[9]), QStringLiteral("ddMMyy"));
188         if (date.isValid())
189             date = date.addYears(100);     // otherwise starts from 1900
190         else
191             date = QDate();
192     }
193 
194     if (parts.count() > 1 && parts[1].count() > 0)
195         QLocationUtils::getNmeaTime(parts[1], &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(parts[3], parts[4][0], parts[5], parts[6][0], &lat, &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(&parsed);
210         if (parsed)
211             info->setAttribute(QGeoPositionInfo::GroundSpeed, qreal(value * 1.852 / 3.6));    // knots -> m/s
212     }
213     if (parts.count() > 8 && parts[8].count() > 0) {
214         value = parts[8].toDouble(&parsed);
215         if (parsed)
216             info->setAttribute(QGeoPositionInfo::Direction, 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(&parsed);
221         if (parsed) {
222             if (parts[11][0] == 'W')
223                 value *= -1;
224             info->setAttribute(QGeoPositionInfo::MagneticVariation, 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 
qlocationutils_readVtg(const char * data,int size,QGeoPositionInfo * info,bool * hasFix)234 static 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(',');
241 
242     bool parsed = false;
243     double value = 0.0;
244     if (parts.count() > 1 && parts[1].count() > 0) {
245         value = parts[1].toDouble(&parsed);
246         if (parsed)
247             info->setAttribute(QGeoPositionInfo::Direction, qreal(value));
248     }
249     if (parts.count() > 7 && parts[7].count() > 0) {
250         value = parts[7].toDouble(&parsed);
251         if (parsed)
252             info->setAttribute(QGeoPositionInfo::GroundSpeed, qreal(value / 3.6));    // km/h -> m/s
253     }
254 }
255 
qlocationutils_readZda(const char * data,int size,QGeoPositionInfo * info,bool * hasFix)256 static 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(',');
263     QDate date;
264     QTime time;
265 
266     if (parts.count() > 1 && parts[1].count() > 0)
267         QLocationUtils::getNmeaTime(parts[1], &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 
getNmeaSentenceType(const char * data,int size)281 QLocationUtils::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 
getPosInfoFromNmea(const char * data,int size,QGeoPositionInfo * info,double uere,bool * hasFix)310 bool 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 
getSatInfoFromNmea(const char * data,int size,QList<QGeoSatelliteInfo> & infos)355 QLocationUtils::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(',');
365 
366     if (parts.count() <= 3) {
367         infos.clear();
368         return GSVFullyParsed; // Malformed sentence.
369     }
370     bool ok;
371     const int totalSentences = parts.at(1).toInt(&ok);
372     if (!ok) {
373         infos.clear();
374         return GSVFullyParsed; // Malformed sentence.
375     }
376 
377     const int sentence = parts.at(2).toInt(&ok);
378     if (!ok) {
379         infos.clear();
380         return GSVFullyParsed; // Malformed sentence.
381     }
382 
383     const int totalSats = parts.at(3).toInt(&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(sentence * 4, 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(field++).toInt(&ok);
398         info.setSatelliteIdentifier((ok) ? prn : 0);
399         const int elevation = parts.at(field++).toInt(&ok);
400         info.setAttribute(QGeoSatelliteInfo::Elevation, (ok) ? elevation : 0);
401         const int azimuth = parts.at(field++).toInt(&ok);
402         info.setAttribute(QGeoSatelliteInfo::Azimuth, (ok) ? azimuth : 0);
403         const int snr = parts.at(field++).toInt(&ok);
404         info.setSignalStrength((ok) ? snr : -1);
405         infos.append(info);
406     }
407 
408     if (sentence == totalSentences)
409         return GSVFullyParsed;
410     return GSVPartiallyParsed;
411 }
412 
getSatInUseFromNmea(const char * data,int size,QList<int> & pnrsInUse)413 bool 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 
hasValidNmeaChecksum(const char * data,int size)434 bool 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,16);
461     return ok && checksum == result;
462 }
463 
getNmeaTime(const QByteArray & bytes,QTime * time)464 bool QLocationUtils::getNmeaTime(const QByteArray &bytes, QTime *time)
465 {
466     int dotIndex = bytes.indexOf('.');
467     QTime tempTime;
468 
469     if (dotIndex < 0) {
470         tempTime = QTime::fromString(QString::fromLatin1(bytes.constData()),
471                                      QStringLiteral("hhmmss"));
472     } else {
473         tempTime = QTime::fromString(QString::fromLatin1(bytes.mid(0, dotIndex)),
474                                      QStringLiteral("hhmmss"));
475         bool hasMsecs = false;
476         int midLen = qMin(3, bytes.size() - dotIndex - 1);
477         int msecs = bytes.mid(dotIndex + 1, midLen).toUInt(&hasMsecs);
478         if (hasMsecs)
479             tempTime = tempTime.addMSecs(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 
getNmeaLatLong(const QByteArray & latString,char latDirection,const QByteArray & lngString,char lngDirection,double * lat,double * lng)489 bool 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(&hasLat);
499     double tempLng = lngString.toDouble(&hasLong);
500     if (hasLat && hasLong) {
501         tempLat = qlocationutils_nmeaDegreesToDecimal(tempLat);
502         if (latDirection == 'S')
503             tempLat *= -1;
504         tempLng = qlocationutils_nmeaDegreesToDecimal(tempLng);
505         if (lngDirection == 'W')
506             tempLng *= -1;
507 
508         if (isValidLat(tempLat) && isValidLong(tempLng)) {
509             *lat = tempLat;
510             *lng = tempLng;
511             return true;
512         }
513     }
514     return false;
515 }
516 
517 QT_END_NAMESPACE
518 
519