1
/***************************************************************************
2
* Copyright (C) 2005 by Robin Gingras *
5
* This program is free software; you can redistribute it and/or modify *
6
* it under the terms of the GNU General Public License as published by *
7
* the Free Software Foundation; either version 2 of the License, or *
8
* (at your option) any later version. *
10
* This program is distributed in the hope that it will be useful, *
11
* but WITHOUT ANY WARRANTY; without even the implied warranty of *
12
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13
* GNU General Public License for more details. *
15
* You should have received a copy of the GNU General Public License *
16
* along with this program; if not, write to the *
17
* Free Software Foundation, Inc., *
18
* 59 Temple Place - Suite 330, Boston, MA 02111-1307, USA. *
19
***************************************************************************/
26
#include <QStringList>
27
#include <QMessageBox>
30
#include <QHostAddress>
34
#include "qgpsdevice.h"
35
#include "qextserialport.h"
37
#include "Preferences/MerkaartorPreferences.h"
41
/* GPSSLOTFORWARDER */
43
GPSSlotForwarder::GPSSlotForwarder(QGPSDevice* aTarget)
48
void GPSSlotForwarder::onLinkReady()
50
Target->onLinkReady();
53
void GPSSlotForwarder::onDataAvailable()
55
Target->onDataAvailable();
58
void GPSSlotForwarder::onStop()
63
void GPSSlotForwarder::checkDataAvailable()
65
Target->checkDataAvailable();
69
* QGPSDevice::QGPSDevice()
71
* Takes in an optional serialPort string and sets the serialPort property
74
* @param char serialPort Serial port to listen to for GPS dat
77
QGPSDevice::QGPSDevice()
79
mutex = new QMutex(QMutex::Recursive);
88
setFixMode(QGPSDevice::FixAuto);
89
setFixType(QGPSDevice::FixUnavailable);
90
setFixStatus(QGPSDevice::StatusVoid);
94
cur_numSatellites = 0;
95
for(int i = 0; i < 50; i ++)
97
satArray[i][0] = satArray[i][1] = satArray[i][2] = 0;
105
int QGPSDevice::latDegrees() { return (int) (fabs(latitude())); }
106
int QGPSDevice::latMinutes()
108
double m = fabs(latitude()) - latDegrees();
111
int QGPSDevice::latSeconds()
113
double m = fabs(latitude()) - latDegrees();
114
double s = (m * 60) - int(m * 60);
117
int QGPSDevice::longDegrees() { return (int) (fabs(longitude())); }
118
int QGPSDevice::longMinutes()
120
double m = fabs(longitude()) - longDegrees();
123
int QGPSDevice::longSeconds()
125
double m = fabs(longitude()) - longDegrees();
126
double s = (m * 60) - int(m * 60);
130
bool QGPSDevice::isActiveSat(unsigned int prn)
132
for (int i=0; i<12; i++) {
133
if (activeSats[i] == prn)
139
void QGPSDevice::satInfo(int index, int &elev, int &azim, int &snr)
143
elev = satArray[index][0];
144
azim = satArray[index][1];
145
snr = satArray[index][2];
153
* Begins the thread loop, reading data from the GPS and parsing
158
* QGPSDevice::parseGGA()
160
* Parses a GPGGA string that contains fix information, such as
161
* latitude, longitude, etc.
163
* The format of the GPGGA String is as follows:
165
* $GPGGA,123519,4807.038,N,01131.000,E,1,08,0.9,545.4,M,46.9,M,,*47
166
* |||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
167
* 01234567890123456789012345678901234567890123456789012345678901234
169
* 0 10 20 30 40 50 60
171
* GPGGA - Global Positioning System Fix Data
172
* 123519 - Fix taken at 12:35:19 UTC
173
* 4807.038,N - Latitude 48 deg 07.038' N
174
* 01131.000,E - Longitude 11 deg 31.000' E
180
* 4 = Real time kinematic
182
* 6 = Estimated (dead reckoning)
183
* 7 = Manual input mode
184
* 8 = Simulation mode
185
* 08 - Number of satellites being tracked
186
* 0.9 - Horizontal dissolution of precision
187
* 545.4,M - Altitude (meters) above sea level
188
* 46.9,M - Height of geoid (sea level) above WGS84 ellipsoid
189
* (empty) - Seconds since last DGPS update
190
* (empty) - DGPS station ID number
191
* *47 - Checksum, begins with *
193
* @param char ggaString The full NMEA GPGGA string, starting with
194
* the $ and ending with the checksum
197
bool QGPSDevice::parseGGA(const char *ggaString)
201
QString line(ggaString);
202
if (line.count('$') > 1)
205
QStringList tokens = line.split(",");
207
double lat = tokens[2].left(2).toDouble();
208
double latmin = tokens[2].mid(2).toDouble();
209
lat += latmin / 60.0;
210
if (tokens[3] != "N")
212
//cur_latitude = lat;
214
if (!tokens[3].isEmpty())
216
if (tokens[3].at(0) == 'N')
217
setLatCardinal(CardinalNorth);
218
else if (tokens[3].at(0) == 'S')
219
setLatCardinal(CardinalSouth);
221
setLatCardinal(CardinalNone);
225
double lon = tokens[4].left(3).toDouble();
226
double lonmin = tokens[4].mid(3).toDouble();
227
lon += lonmin / 60.0;
228
if (tokens[5] != "E")
230
//cur_longitude = lon;
232
if (!tokens[5].isEmpty())
234
if (tokens[5].at(0) == 'E')
235
setLatCardinal(CardinalEast);
236
else if (tokens[5].at(0) == 'W')
237
setLatCardinal(CardinalWest);
239
setLatCardinal(CardinalNone);
242
int fix = tokens[6].toInt();
245
int numSat = tokens[7].toInt();
246
setNumSatellites(numSat);
248
float dilut = tokens[8].toFloat();
251
float altitude = tokens[9].toFloat();
252
setAltitude(altitude);
259
bool QGPSDevice::parseGLL(const char *ggaString)
263
QString line(ggaString);
264
if (line.count('$') > 1)
267
QStringList tokens = line.split(",");
269
double lat = tokens[1].left(2).toDouble();
270
double latmin = tokens[1].mid(2).toDouble();
271
lat += latmin / 60.0;
272
if (tokens[2] != "N")
274
//cur_latitude = lat;
276
if (!tokens[2].isEmpty())
278
if (tokens[2].at(0) == 'N')
279
setLatCardinal(CardinalNorth);
280
else if (tokens[2].at(0) == 'S')
281
setLatCardinal(CardinalSouth);
283
setLatCardinal(CardinalNone);
286
double lon = tokens[3].left(3).toDouble();
287
double lonmin = tokens[3].mid(3).toDouble();
288
lon += lonmin / 60.0;
289
if (tokens[4] != "E")
291
//cur_longitude = lon;
293
if (!tokens[4].isEmpty())
295
if (tokens[4].at(0) == 'E')
296
setLatCardinal(CardinalEast);
297
else if (tokens[4].at(0) == 'W')
298
setLatCardinal(CardinalWest);
300
setLatCardinal(CardinalNone);
304
if (tokens[6] == "A")
306
setFixStatus(StatusActive);
310
setFixStatus(StatusVoid);
319
* QGPSDevice::parseGSA()
321
* Parses a GPGSA string that contains information about the nature
322
* of the fix, such as DOP (dillution of precision) and active satellites
323
* based on the viewing mask and almanac data of the reciever.
325
* The format of the GPGSA String is as follows:
327
* $GPGSA,A,3,04,05,,09,12,,,24,,,,,2.5,1.3,2.1*39
328
* |||||||||||||||||||||||||||||||||||||||||||||||
329
* 01234567890123456789012345678901234567890123456
333
* GPGSA - Information about satellite status
334
* A - Fix mode, (A)utomatic or (M)anual
338
* 3 = 3D (4 or more satellites)
339
* 04,05,... - Satellites used in the solution (up to 12)
340
* 2.5 - DOP (dillution of precision)
341
* 1.3 - Horizontal DOP
345
* @param char The full NMEA GPGSA string, from $ to checksum
348
bool QGPSDevice::parseGSA(const char *gsaString)
352
QString line(gsaString);
353
if (line.count('$') > 1)
356
QStringList tokens = line.split(",");
358
QString autoSelectFix = tokens[1];
359
if(autoSelectFix == "A")
362
setFixMode(FixManual);
364
int fix = tokens[2].toInt();
366
setFixType(FixInvalid);
372
for(int index = 0; index < 12; index ++) {
373
activeSats[index] = tokens[index+3].toInt();
382
* QGPSDevice::parseRMC()
384
* Parses an RMC string, which contains the recommended minimum fix
385
* data, such as latitude, longitude, altitude, speed, track angle,
386
* date, and magnetic variation. Saves us some calculating :)
388
* The format of the GPRMC string is as follows:
390
* $GPRMC,123519,A,4807.038,N,01131.000,E,022.4,084.4,230394,003.1,W*6A
391
* ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
392
* 01234567890123456789012345678901234567890123456789012345678901234567
394
* 0 10 20 30 40 50 60
396
* GPRMC - Recommended minimum fix data
397
* 123519 - Fix taken at 12:35:19 UTC
398
* A - Fix status, (A)ctive or (V)oid
399
* 4807.038,N - Latitude 48 degrees 07.038' N
400
* 01131.000,E - Longitude 11 degrees, 31.000' E
401
* 022.4 - Ground speed in knots
402
* 084.4 - Track angle in degrees (true north)
403
* 230394 - Date: 23 March 1994
404
* 003.1,W - Magnetic Variation
407
* @param char Full RMC string, from $ to checksum
410
bool QGPSDevice::parseRMC(const char *rmcString)
416
QString line(rmcString);
417
if (line.count('$') > 1)
420
QStringList tokens = line.split(",");
422
QString strDate = tokens[9] + tokens[1];
423
cur_datetime = QDateTime::fromString(strDate, "ddMMyyHHmmss.zzz");
425
if (cur_datetime.date().year() < 1970)
426
cur_datetime = cur_datetime.addYears(100);
430
if (tokens[2] == "A")
432
setFixStatus(StatusActive);
436
setFixStatus(StatusVoid);
441
double lat = tokens[3].left(2).toDouble();
442
double latmin = tokens[3].mid(2).toDouble();
443
lat += latmin / 60.0;
444
if (tokens[4] != "N")
448
if (!tokens[4].isEmpty())
450
if (tokens[4].at(0) == 'N')
451
setLatCardinal(CardinalNorth);
452
else if (tokens[4].at(0) == 'S')
453
setLatCardinal(CardinalSouth);
455
setLatCardinal(CardinalNone);
458
double lon = tokens[5].left(3).toDouble();
459
double lonmin = tokens[5].mid(3).toDouble();
460
lon += lonmin / 60.0;
461
if (tokens[6] != "E")
465
if (!tokens[6].isEmpty())
467
if (tokens[6].at(0) == 'E')
468
setLatCardinal(CardinalEast);
469
else if (tokens[6].at(0) == 'W')
470
setLatCardinal(CardinalWest);
472
setLatCardinal(CardinalNone);
475
// Ground speed in km/h
477
double speed = QString::number(tokens[7].toDouble() * 1.852, 'f', 1).toDouble();
482
double heading = tokens[8].toDouble();
485
// Magnetic variation
487
double magvar = tokens[10].toDouble();
488
setVariation(magvar);
490
if (!tokens[11].isEmpty())
492
if (tokens[11].at(0) == 'E')
493
setVarCardinal(CardinalEast);
494
else if (tokens[11].at(0) == 'W')
495
setVarCardinal(CardinalWest);
497
setVarCardinal(CardinalNone);
506
* QGPSDevice::parseGSV()
508
* Parses a GPGSV string, which contains satellite position and signal
509
* strenght information. parseGSV() fills the satArray array with the
510
* PRNs, elevations, azimuths, and SNRs of the visible satellites. This
511
* array is based on the position of the satellite in the strings, not
512
* the order of the PRNs! (see README for info)
514
* The format of the GPGSV string is as follows:
516
* $GPGSV,2,1,08,01,40,083,46,02,17,308,41,12,07,344,39,14,22,228,45*75
517
* ||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||||
518
* 01234567890123456789012345678901234567890123456789012345678901234567
520
* 0 10 20 30 40 50 60
522
* GPGSV - Satellite status
523
* 2 - Number of GPGSV sentences for full data
524
* 1 - Current sentence (1 of 2, etc)
525
* 08 - Number of satellites in view
528
* 40 - Elevation, degrees
529
* 083 - Azimuth, degrees
530
* 46 - SNR (signal to noise ratio)
531
* (for up to four satellites per sentence)
535
bool QGPSDevice::parseGSV(const char *gsvString)
542
int prn, elev, azim, snr;
544
QString line(gsvString);
545
if (line.count('$') > 1)
548
QStringList tokens = line.split(",");
550
totalSentences = tokens[1].toInt();
551
currentSentence = tokens[2].toInt();
552
totalSatellites = tokens[3].toInt();
554
for(int i = 0; (i < 4) && ((i*4)+4+3 < tokens.size()); i ++) {
555
prn = tokens[(i*4)+4].toInt();
556
elev = tokens[(i*4)+4+1].toInt();
557
azim = tokens[(i*4)+4+2].toInt();
558
if (tokens[(i*4)+4+3].contains('*')) {
559
QStringList tok2 = tokens[(i*4)+4+3].split("*");
560
snr = tok2[0].toInt();
562
snr = tokens[(i*4)+4+3].toInt();
563
satArray[prn][0] = elev;
564
satArray[prn][1] = azim;
565
satArray[prn][2] = snr;
574
* QGPSDevice::startDevice()
576
* Calls start() to begin thread execution
579
void QGPSDevice::startDevice()
585
printf("We're starting...\n");
591
* QGPSDevice::stopDevice()
593
* Stops execution of run() and ends thread execution
594
* This function will be called outside this thread
597
void QGPSDevice::stopDevice()
599
// this is through a queued connection
603
/*** QGPSComDevice ***/
605
QGPSComDevice::QGPSComDevice(const QString &device)
606
: QGPSDevice(), LogFile(0)
614
QGPSComDevice::~QGPSComDevice()
617
if (LogFile->isOpen())
624
* QGPSComDevice::openDevice()
626
* Opens the serial port and sets the parameters for data transfer: parity,
627
* stop bits, blocking, etc.
630
bool QGPSComDevice::openDevice()
632
port = new QextSerialPort(device());
633
port->setBaudRate(BAUD4800);
634
port->setFlowControl(FLOW_OFF);
635
port->setParity(PAR_NONE);
636
port->setDataBits(DATA_8);
637
port->setStopBits(STOP_2);
639
if (port->open(QIODevice::ReadOnly)) {
640
if (M_PREFS->getGpsSaveLog()) {
641
QString fn = "log-" + QDateTime::currentDateTime().toString(Qt::ISODate) + ".nmea";
642
fn.replace(':', '-');
643
LogFile = new QFile(M_PREFS->getGpsLogDir() + "/"+fn);
644
if (!LogFile->open(QIODevice::WriteOnly)) {
645
QMessageBox::critical(NULL, tr("GPS log error"),
646
tr("Unable to create GPS log file: %1.").arg(M_PREFS->getGpsLogDir() + "/"+fn), QMessageBox::Ok);
657
* QGPSComDevice::closeDevice()
659
* Closes the serial port
662
bool QGPSComDevice::closeDevice()
665
if (LogFile && LogFile->isOpen()) {
674
void QGPSComDevice::onLinkReady()
678
void QGPSComDevice::onStop()
684
void QGPSComDevice::run()
686
GPSSlotForwarder Forward(this);
688
QTimer *timer = new QTimer(this);
689
connect(timer, SIGNAL(timeout()), &Forward, SLOT(checkDataAvailable()));
692
// connect(port,SIGNAL(readyRead()),&Forward,SLOT(onDataAvailable()));
693
connect(this,SIGNAL(doStopDevice()),&Forward,SLOT(onStop()));
698
void QGPSComDevice::checkDataAvailable() {
699
if (port->bytesAvailable() > 0)
703
void QGPSComDevice::onDataAvailable()
705
QByteArray ba(port->readAll());
706
// filter out unwanted characters
707
for (unsigned int i=ba.count(); i; --i)
708
if(ba[i-1] == '\0' ||
709
(!isalnum((quint8)ba[i-1]) &&
710
!isspace((quint8)ba[i-1]) &&
711
!ispunct((quint8)ba[i-1])))
718
if (Buffer.length() > 4096)
720
Buffer.remove(0,Buffer.length()-4096);
721
while (Buffer.count())
723
// look for begin of sentence marker
724
int i = Buffer.indexOf('$');
731
// look for end of sentence marker
732
for (i=0; i<Buffer.count(); ++i)
733
if ( (Buffer[i] == (char)(0x0a)) || (Buffer[i] == (char)(0x0d)) )
735
if (i == Buffer.count())
737
parse(Buffer.mid(0,i-2));
743
void QGPSComDevice::parse(const QByteArray& bufferString)
745
if (bufferString.length() < 6) return;
746
if(bufferString[3] == 'G' && bufferString[4] == 'G' && bufferString[5] == 'A')
748
//strcpy(nmeastr_gga, bufferString);
749
parseGGA(bufferString.data());
751
else if(bufferString[3] == 'G' && bufferString[4] == 'L' && bufferString[5] == 'L')
753
//strcpy(nmeastr_gga, bufferString);
754
parseGLL(bufferString.data());
756
else if(bufferString[3] == 'G' && bufferString[4] == 'S' && bufferString[5] == 'V')
758
//strcpy(nmeastr_gsv, bufferString);
759
parseGSV(bufferString.data());
761
else if(bufferString[3] == 'G' && bufferString[4] == 'S' && bufferString[5] == 'A')
763
//strcpy(nmeastr_gsa, bufferString);
764
parseGSA(bufferString.data());
766
else if(bufferString[3] == 'R' && bufferString[4] == 'M' && bufferString[5] == 'C')
768
//strcpy(nmeastr_rmc, bufferString);
769
if (parseRMC(bufferString.data()))
770
if (fixStatus() == QGPSDevice::StatusActive && (fixType() == QGPSDevice::Fix3D || fixType() == QGPSDevice::FixUnavailable))
771
emit updatePosition(latitude(), longitude(), dateTime(), altitude(), speed(), heading());
776
/*** QGPSFileDevice ***/
778
QGPSFileDevice::QGPSFileDevice(const QString &device)
788
* QGPSFileDevice::openDevice()
790
* Opens the serial port and sets the parameters for data transfer: parity,
791
* stop bits, blocking, etc.
794
bool QGPSFileDevice::openDevice()
796
theFile = new QFile(device());
798
if (!theFile->open(QIODevice::ReadOnly | QIODevice::Text)) {
807
void QGPSFileDevice::onLinkReady()
811
void QGPSFileDevice::onStop()
817
* QGPSFileDevice::closeDevice()
819
* Closes the serial port
822
bool QGPSFileDevice::closeDevice()
830
void QGPSFileDevice::run()
832
GPSSlotForwarder Forward(this);
833
QTimer* t = new QTimer;
834
connect(t,SIGNAL(timeout()),&Forward,SLOT(onDataAvailable()));
835
connect(this,SIGNAL(doStopDevice()),&Forward,SLOT(onStop()));
841
void QGPSFileDevice::onDataAvailable()
846
char bufferString[100];
848
theFile->read(&bufferChar, 1);
849
if(bufferChar == '$')
852
bufferString[index] = bufferChar;
856
theFile->read(&bufferChar, 1);
857
if(bufferChar != '\0' && (isalnum(bufferChar) || isspace(bufferChar) || ispunct(bufferChar)))
860
bufferString[index] = bufferChar;
862
} while(bufferChar != 0x0a && bufferChar != 0x0d);
864
bufferString[index + 1] = '\0';
868
if(bufferString[3] == 'G' && bufferString[4] == 'G' && bufferString[5] == 'A')
870
//strcpy(nmeastr_gga, bufferString);
871
parseGGA(bufferString);
873
else if(bufferString[3] == 'G' && bufferString[4] == 'L' && bufferString[5] == 'L')
875
//strcpy(nmeastr_gga, bufferString);
876
parseGLL(bufferString);
878
else if(bufferString[3] == 'G' && bufferString[4] == 'S' && bufferString[5] == 'V')
880
//strcpy(nmeastr_gsv, bufferString);
881
parseGSV(bufferString);
883
else if(bufferString[3] == 'G' && bufferString[4] == 'S' && bufferString[5] == 'A')
885
//strcpy(nmeastr_gsa, bufferString);
886
parseGSA(bufferString);
888
else if(bufferString[3] == 'R' && bufferString[4] == 'M' && bufferString[5] == 'C')
890
//strcpy(nmeastr_rmc, bufferString);
891
if (parseRMC(bufferString))
892
if (fixStatus() == QGPSDevice::StatusActive && (fixType() == QGPSDevice::Fix3D || fixType() == QGPSDevice::FixUnavailable))
893
emit updatePosition(latitude(), longitude(), dateTime(), altitude(), speed(), heading());
904
QGPSDDevice::QGPSDDevice(const QString& device)
909
bool QGPSDDevice::openDevice()
914
bool QGPSDDevice::closeDevice()
920
// this function will be called within this thread
921
void QGPSDDevice::onStop()
926
void QGPSDDevice::run()
928
GPSSlotForwarder Forward(this);
931
Link.connectToHost(QHostAddress("127.0.0.1"),2947);
932
connect(Server,SIGNAL(connected()),&Forward,SLOT(onLinkReady()));
933
connect(Server,SIGNAL(readyRead()),&Forward,SLOT(onDataAvailable()));
934
connect(this,SIGNAL(doStopDevice()),&Forward,SLOT(onStop()));
938
void QGPSDDevice::onDataAvailable()
940
QByteArray ba(Server->readAll());
942
if (Buffer.length() > 4096)
944
Buffer.remove(0,Buffer.length()-4096);
945
int i = Buffer.indexOf("GPSD");
948
int j = Buffer.indexOf(10,i);
951
parse(QString::fromAscii(Buffer.data()+(i+5),(j-i-6)));
952
Buffer.remove(0,j+1);
955
void QGPSDDevice::parse(const QString& s)
957
std::cout << "parsing " << s.toUtf8().data() << "*" << std::endl;
958
QStringList Args(s.split(',',QString::SkipEmptyParts));
959
for (int i=0; i<Args.count(); ++i)
961
QString Left(Args[i].left(2));
963
parseO(Args[i].right(Args[i].length()-2));
965
parseY(Args[i].right(Args[i].length()-2));
969
void QGPSDDevice::parseY(const QString& s)
971
for(int i = 0; i < 50; i ++)
972
satArray[i][0] = satArray[i][1] = satArray[i][2] = 0;
973
QStringList Sats(s.split(':',QString::SkipEmptyParts));
974
for (int i=1; i<Sats.size(); ++i)
976
QStringList Items(Sats[i].split(' ',QString::SkipEmptyParts));
977
if (Items.count() < 5)
979
int id = Items[0].toInt();
980
if ( (id >= 0) && (id<50) )
982
satArray[id][0] = int(Items[1].toDouble());
983
satArray[id][1] = int(Items[2].toDouble());
984
satArray[id][2] = int(Items[3].toDouble());
987
setNumSatellites(Sats.size());
991
void QGPSDDevice::parseO(const QString& s)
993
if (s.isEmpty()) return;
994
setFixType(FixInvalid);
995
if (s[0] == '?') return;
996
QStringList Args(s.split(' ',QString::SkipEmptyParts));
997
if (Args.count() < 5) return;
999
setLatitude(Args[3].toDouble());
1000
setLongitude(Args[4].toDouble());
1002
if (Args.count() > 5)
1003
Alt = Args[5].toDouble();
1005
if (Args.count() > 9)
1006
Speed = Args[9].toDouble();
1008
if (Args.count() > 7)
1009
Heading = Args[7].toDouble();
1010
emit updatePosition(Args[3].toDouble(),
1012
QDateTime::currentDateTime(),
1013
Alt, Speed, Heading);
1014
setHeading(Heading);
1017
emit updateStatus();
1021
void QGPSDDevice::onLinkReady()
1023
if (!Server) return;
1024
Server->write("w+");
1025
Server->write("j=1");
added
removed
GPS/qgpsdevice.cpp
