Branch data Line data Source code
1 : : /***************************************************************************
2 : : qgsnmeaconnection.cpp - description
3 : : ---------------------
4 : : begin : November 30th, 2009
5 : : copyright : (C) 2009 by Marco Hugentobler
6 : : email : marco at hugis dot net
7 : : ***************************************************************************/
8 : :
9 : : /***************************************************************************
10 : : * *
11 : : * This program is free software; you can redistribute it and/or modify *
12 : : * it under the terms of the GNU General Public License as published by *
13 : : * the Free Software Foundation; either version 2 of the License, or *
14 : : * (at your option) any later version. *
15 : : * *
16 : : ***************************************************************************/
17 : :
18 : : #include "qgsnmeaconnection.h"
19 : : #include "qgslogger.h"
20 : :
21 : : #include <QIODevice>
22 : : #include <QApplication>
23 : : #include <QStringList>
24 : :
25 : :
26 : : //from libnmea
27 : : #include "parse.h"
28 : : #include "gmath.h"
29 : : #include "info.h"
30 : :
31 : : // for sqrt
32 : : #include <math.h>
33 : :
34 : : #define KNOTS_TO_KMH 1.852
35 : :
36 : 0 : QgsNmeaConnection::QgsNmeaConnection( QIODevice *device )
37 : 0 : : QgsGpsConnection( device )
38 : 0 : {
39 : 0 : }
40 : :
41 : 0 : void QgsNmeaConnection::parseData()
42 : : {
43 : 0 : if ( !mSource )
44 : : {
45 : 0 : return;
46 : : }
47 : :
48 : : //print out the data as a test
49 : 0 : qint64 numBytes = 0;
50 : 0 : if ( ! mSource->isSequential() ) //necessary because of a bug in QExtSerialPort //SLM - bytesAvailable() works on Windows, so I reversed the logic (added ! ); this is what QIODevice docs say to do; the orig impl of win_qextserialport had an (unsigned int)-1 return on error - it should be (qint64)-1, which was fixed by ?
51 : : {
52 : 0 : numBytes = mSource->size();
53 : 0 : }
54 : : else
55 : : {
56 : 0 : numBytes = mSource->bytesAvailable();
57 : : }
58 : :
59 : 0 : QgsDebugMsgLevel( "numBytes:" + QString::number( numBytes ), 2 );
60 : :
61 : 0 : if ( numBytes >= 6 )
62 : : {
63 : 0 : if ( mStatus != GPSDataReceived )
64 : : {
65 : 0 : mStatus = DataReceived;
66 : 0 : }
67 : :
68 : : //append new data to the remaining results from last parseData() call
69 : 0 : mStringBuffer.append( mSource->read( numBytes ) );
70 : 0 : processStringBuffer();
71 : 0 : emit stateChanged( mLastGPSInformation );
72 : 0 : }
73 : 0 : }
74 : :
75 : 0 : void QgsNmeaConnection::processStringBuffer()
76 : : {
77 : 0 : int endSentenceIndex = 0;
78 : : int dollarIndex;
79 : :
80 : 0 : while ( ( endSentenceIndex = mStringBuffer.indexOf( QLatin1String( "\r\n" ) ) ) && endSentenceIndex != -1 )
81 : : {
82 : 0 : endSentenceIndex = mStringBuffer.indexOf( QLatin1String( "\r\n" ) );
83 : :
84 : 0 : dollarIndex = mStringBuffer.indexOf( QLatin1Char( '$' ) );
85 : 0 : if ( endSentenceIndex == -1 )
86 : : {
87 : 0 : break;
88 : : }
89 : :
90 : :
91 : 0 : if ( endSentenceIndex >= dollarIndex )
92 : : {
93 : 0 : if ( dollarIndex != -1 )
94 : : {
95 : 0 : QString substring = mStringBuffer.mid( dollarIndex, endSentenceIndex );
96 : 0 : QByteArray ba = substring.toLocal8Bit();
97 : 0 : if ( substring.startsWith( QLatin1String( "$GPGGA" ) ) || substring.startsWith( QLatin1String( "$GNGGA" ) ) )
98 : : {
99 : 0 : QgsDebugMsgLevel( substring, 2 );
100 : 0 : processGgaSentence( ba.data(), ba.length() );
101 : 0 : mStatus = GPSDataReceived;
102 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
103 : 0 : }
104 : 0 : else if ( substring.startsWith( QLatin1String( "$GPRMC" ) ) || substring.startsWith( QLatin1String( "$GNRMC" ) ) )
105 : : {
106 : 0 : QgsDebugMsgLevel( substring, 2 );
107 : 0 : processRmcSentence( ba.data(), ba.length() );
108 : 0 : mStatus = GPSDataReceived;
109 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
110 : 0 : }
111 : 0 : else if ( substring.startsWith( QLatin1String( "$GPGSV" ) ) || substring.startsWith( QLatin1String( "$GNGSV" ) ) )
112 : : {
113 : 0 : QgsDebugMsgLevel( substring, 2 );
114 : 0 : processGsvSentence( ba.data(), ba.length() );
115 : 0 : mStatus = GPSDataReceived;
116 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
117 : 0 : }
118 : 0 : else if ( substring.startsWith( QLatin1String( "$GPVTG" ) ) || substring.startsWith( QLatin1String( "$GNVTG" ) ) )
119 : : {
120 : 0 : QgsDebugMsgLevel( substring, 2 );
121 : 0 : processVtgSentence( ba.data(), ba.length() );
122 : 0 : mStatus = GPSDataReceived;
123 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
124 : 0 : }
125 : 0 : else if ( substring.startsWith( QLatin1String( "$GPGSA" ) ) || substring.startsWith( QLatin1String( "$GNGSA" ) ) )
126 : : {
127 : 0 : QgsDebugMsgLevel( substring, 2 );
128 : 0 : processGsaSentence( ba.data(), ba.length() );
129 : 0 : mStatus = GPSDataReceived;
130 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
131 : 0 : }
132 : 0 : else if ( substring.startsWith( QLatin1String( "$GPGST" ) ) || substring.startsWith( QLatin1String( "$GNGST" ) ) )
133 : : {
134 : 0 : QgsDebugMsgLevel( substring, 2 );
135 : 0 : processGstSentence( ba.data(), ba.length() );
136 : 0 : mStatus = GPSDataReceived;
137 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
138 : 0 : }
139 : 0 : else if ( substring.startsWith( QLatin1String( "$GPHDT" ) ) || substring.startsWith( QLatin1String( "$GNHDT" ) ) )
140 : : {
141 : 0 : QgsDebugMsgLevel( substring, 2 );
142 : 0 : processHdtSentence( ba.data(), ba.length() );
143 : 0 : mStatus = GPSDataReceived;
144 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
145 : 0 : }
146 : 0 : else if ( substring.startsWith( QLatin1String( "$HCHDG" ) ) )
147 : : {
148 : 0 : QgsDebugMsgLevel( substring, 2 );
149 : 0 : processHchdgSentence( ba.data(), ba.length() );
150 : 0 : mStatus = GPSDataReceived;
151 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
152 : 0 : }
153 : 0 : else if ( substring.startsWith( QLatin1String( "$HCHDT" ) ) )
154 : : {
155 : 0 : QgsDebugMsgLevel( substring, 2 );
156 : 0 : processHchdtSentence( ba.data(), ba.length() );
157 : 0 : mStatus = GPSDataReceived;
158 : 0 : QgsDebugMsgLevel( QStringLiteral( "*******************GPS data received****************" ), 2 );
159 : 0 : }
160 : : else
161 : : {
162 : 0 : QgsDebugMsgLevel( QStringLiteral( "unknown nmea sentence: %1" ).arg( substring ), 2 );
163 : : }
164 : 0 : emit nmeaSentenceReceived( substring ); // added to be able to save raw data
165 : 0 : }
166 : 0 : }
167 : 0 : mStringBuffer.remove( 0, endSentenceIndex + 2 );
168 : : }
169 : 0 : }
170 : :
171 : 0 : void QgsNmeaConnection::processGgaSentence( const char *data, int len )
172 : : {
173 : : nmeaGPGGA result;
174 : 0 : if ( nmea_parse_GPGGA( data, len, &result ) )
175 : : {
176 : : //update mLastGPSInformation
177 : 0 : double longitude = result.lon;
178 : 0 : if ( result.ew == 'W' )
179 : : {
180 : 0 : longitude = -longitude;
181 : 0 : }
182 : 0 : double latitude = result.lat;
183 : 0 : if ( result.ns == 'S' )
184 : : {
185 : 0 : latitude = -latitude;
186 : 0 : }
187 : :
188 : 0 : mLastGPSInformation.longitude = nmea_ndeg2degree( longitude );
189 : 0 : mLastGPSInformation.latitude = nmea_ndeg2degree( latitude );
190 : 0 : mLastGPSInformation.elevation = result.elv;
191 : 0 : mLastGPSInformation.elevation_diff = result.diff;
192 : 0 : mLastGPSInformation.quality = result.sig;
193 : 0 : mLastGPSInformation.satellitesUsed = result.satinuse;
194 : 0 : }
195 : 0 : }
196 : :
197 : 0 : void QgsNmeaConnection::processGstSentence( const char *data, int len )
198 : : {
199 : : nmeaGPGST result;
200 : 0 : if ( nmea_parse_GPGST( data, len, &result ) )
201 : : {
202 : : //update mLastGPSInformation
203 : 0 : double sig_lat = result.sig_lat;
204 : 0 : double sig_lon = result.sig_lon;
205 : 0 : double sig_alt = result.sig_alt;
206 : :
207 : : // Horizontal RMS
208 : 0 : mLastGPSInformation.hacc = sqrt( ( pow( sig_lat, 2 ) + pow( sig_lon, 2 ) ) / 2.0 );
209 : : // Vertical RMS
210 : 0 : mLastGPSInformation.vacc = sig_alt;
211 : : // 3D RMS
212 : 0 : mLastGPSInformation.hvacc = sqrt( ( pow( sig_lat, 2 ) + pow( sig_lon, 2 ) + pow( sig_alt, 2 ) ) / 3.0 );
213 : 0 : }
214 : 0 : }
215 : :
216 : 0 : void QgsNmeaConnection::processHdtSentence( const char *data, int len )
217 : : {
218 : : nmeaGPHDT result;
219 : 0 : if ( nmea_parse_GPHDT( data, len, &result ) )
220 : : {
221 : 0 : mLastGPSInformation.direction = result.heading;
222 : 0 : }
223 : 0 : }
224 : :
225 : 0 : void QgsNmeaConnection::processHchdgSentence( const char *data, int len )
226 : : {
227 : : nmeaHCHDG result;
228 : 0 : if ( nmea_parse_HCHDG( data, len, &result ) )
229 : : {
230 : 0 : mLastGPSInformation.direction = result.mag_heading;
231 : 0 : if ( result.ew_variation == 'E' )
232 : 0 : mLastGPSInformation.direction += result.mag_variation;
233 : : else
234 : 0 : mLastGPSInformation.direction -= result.mag_variation;
235 : 0 : }
236 : 0 : }
237 : :
238 : 0 : void QgsNmeaConnection::processHchdtSentence( const char *data, int len )
239 : : {
240 : : nmeaHCHDT result;
241 : 0 : if ( nmea_parse_HCHDT( data, len, &result ) )
242 : : {
243 : 0 : mLastGPSInformation.direction = result.direction;
244 : 0 : }
245 : 0 : }
246 : :
247 : 0 : void QgsNmeaConnection::processRmcSentence( const char *data, int len )
248 : : {
249 : : nmeaGPRMC result;
250 : 0 : if ( nmea_parse_GPRMC( data, len, &result ) )
251 : : {
252 : 0 : double longitude = result.lon;
253 : 0 : if ( result.ew == 'W' )
254 : : {
255 : 0 : longitude = -longitude;
256 : 0 : }
257 : 0 : double latitude = result.lat;
258 : 0 : if ( result.ns == 'S' )
259 : : {
260 : 0 : latitude = -latitude;
261 : 0 : }
262 : 0 : mLastGPSInformation.longitude = nmea_ndeg2degree( longitude );
263 : 0 : mLastGPSInformation.latitude = nmea_ndeg2degree( latitude );
264 : 0 : mLastGPSInformation.speed = KNOTS_TO_KMH * result.speed;
265 : 0 : if ( !std::isnan( result.direction ) )
266 : 0 : mLastGPSInformation.direction = result.direction;
267 : 0 : mLastGPSInformation.status = result.status; // A,V
268 : :
269 : : //date and time
270 : 0 : QDate date( result.utc.year + 1900, result.utc.mon + 1, result.utc.day );
271 : 0 : QTime time( result.utc.hour, result.utc.min, result.utc.sec, result.utc.msec ); // added msec part
272 : 0 : if ( date.isValid() && time.isValid() )
273 : : {
274 : 0 : mLastGPSInformation.utcDateTime.setTimeSpec( Qt::UTC );
275 : 0 : mLastGPSInformation.utcDateTime.setDate( date );
276 : 0 : mLastGPSInformation.utcDateTime.setTime( time );
277 : 0 : QgsDebugMsgLevel( QStringLiteral( "utc time:" ), 2 );
278 : 0 : QgsDebugMsgLevel( mLastGPSInformation.utcDateTime.toString(), 2 );
279 : 0 : QgsDebugMsgLevel( QStringLiteral( "local time:" ), 2 );
280 : 0 : QgsDebugMsgLevel( mLastGPSInformation.utcDateTime.toLocalTime().toString(), 2 );
281 : 0 : }
282 : 0 : }
283 : 0 : }
284 : :
285 : 0 : void QgsNmeaConnection::processGsvSentence( const char *data, int len )
286 : : {
287 : : nmeaGPGSV result;
288 : 0 : if ( nmea_parse_GPGSV( data, len, &result ) )
289 : : {
290 : : //clear satellite information when a new series of packs arrives
291 : 0 : if ( result.pack_index == 1 )
292 : : {
293 : 0 : mLastGPSInformation.satellitesInView.clear();
294 : 0 : }
295 : :
296 : : // for determining when to graph sat info
297 : 0 : mLastGPSInformation.satInfoComplete = ( result.pack_index == result.pack_count );
298 : :
299 : 0 : for ( int i = 0; i < NMEA_SATINPACK; ++i )
300 : : {
301 : 0 : nmeaSATELLITE currentSatellite = result.sat_data[i];
302 : 0 : QgsSatelliteInfo satelliteInfo;
303 : 0 : satelliteInfo.azimuth = currentSatellite.azimuth;
304 : 0 : satelliteInfo.elevation = currentSatellite.elv;
305 : 0 : satelliteInfo.id = currentSatellite.id;
306 : 0 : satelliteInfo.inUse = currentSatellite.in_use; // the GSA processing below does NOT set the sats in use
307 : 0 : satelliteInfo.signal = currentSatellite.sig;
308 : 0 : mLastGPSInformation.satellitesInView.append( satelliteInfo );
309 : 0 : }
310 : :
311 : 0 : }
312 : 0 : }
313 : :
314 : 0 : void QgsNmeaConnection::processVtgSentence( const char *data, int len )
315 : : {
316 : : nmeaGPVTG result;
317 : 0 : if ( nmea_parse_GPVTG( data, len, &result ) )
318 : : {
319 : 0 : mLastGPSInformation.speed = result.spk;
320 : 0 : }
321 : 0 : }
322 : :
323 : 0 : void QgsNmeaConnection::processGsaSentence( const char *data, int len )
324 : : {
325 : : nmeaGPGSA result;
326 : 0 : if ( nmea_parse_GPGSA( data, len, &result ) )
327 : : {
328 : 0 : mLastGPSInformation.satPrn.clear();
329 : 0 : mLastGPSInformation.hdop = result.HDOP;
330 : 0 : mLastGPSInformation.pdop = result.PDOP;
331 : 0 : mLastGPSInformation.vdop = result.VDOP;
332 : 0 : mLastGPSInformation.fixMode = result.fix_mode;
333 : 0 : mLastGPSInformation.fixType = result.fix_type;
334 : 0 : for ( int i = 0; i < NMEA_MAXSAT; i++ )
335 : : {
336 : 0 : mLastGPSInformation.satPrn.append( result.sat_prn[ i ] );
337 : 0 : }
338 : 0 : }
339 : 0 : }
|
