source: pacpussensors/trunk/Gps/gpsComponent.cpp@ 106

Last change on this file since 106 was 99, checked in by nguyenhu, 9 years ago

compilation under linux with 0.2.X framework

File size: 24.2 KB
Line 
1/// Purpose: Implementation of the GpsComponent class
2///
3/// @date created 2005/09/02 - 14:16
4/// @author Gerald Dherbomez
5/// @copyright Heudiasyc UMR UTC/CNRS 6599
6/// @version $Id: gpsComponent.cpp 1278 2013-01-16 16:40:11Z bonnetst $
7
8#include "gpsComponent.h"
9
10#include <qfile.h>
11#include <qapplication.h>
12
13#include "Pacpus/kernel/ComponentFactory.h"
14#include "Pacpus/kernel/Log.h"
15#include "Pacpus/kernel/DbiteFileTypes.h"
16
17#include <qdebug.h>
18#include <iostream>
19
20#define PACPUS_PI 3.1415926
21
22namespace pacpus {
23
24/// Construct the factory
25ComponentFactory<GpsComponent> sFactory("GpsComponent");
26
27DECLARE_STATIC_LOGGER("pacpus.base.GpsComponent");
28
29#define UNKNOWN_NMEA_FRAME -1
30
31//////////////////////////////////////////////////////////////////////////
32/*!
33* Constructor of GpsComponent class
34*/
35//////////////////////////////////////////////////////////////////////////
36GpsComponent::GpsComponent(QString name)
37 : semaphore_(0)
38 , ComponentBase(name)
39
40{
41
42 ppsRecording = true;//TRUE
43 ggaRecording = true;
44 gsaRecording = true;
45 gstRecording = true;
46 gsvRecording = true;
47 hdtRecording = true;
48 rmcRecording = true;
49 rotRecording = true;
50 vtgRecording = true;
51 zdaRecording = true;
52
53/* ppshdFile = NULL;
54 ggahdFile = NULL;
55 gsahdFile = NULL;
56 gsthdFile = NULL;
57 gsvhdFile = NULL;
58 hdthdFile = NULL;
59 rmchdFile = NULL;
60 rothdFile = NULL;
61 vtghdFile = NULL;
62 zdahdFile = NULL;
63*/
64
65
66 nextByteToProcess_ = 0;
67 currentFrame_ = NULL;
68 newFrameToDecode_ = false;
69 startOfFrame_ = false;
70 endOfFrame_ = false;
71}
72
73
74/*!
75* Destructor of the GpsComponent class
76*/
77GpsComponent::~GpsComponent()
78{
79
80}
81
82
83ComponentBase::COMPONENT_CONFIGURATION GpsComponent::configureComponent(XmlComponentConfig config)
84{
85 setPortCOM( config.getProperty("port").toLatin1() );
86 setRecording ((config.getProperty("recording") == "true" ? true : false));
87
88 return ComponentBase::CONFIGURED_OK;
89}
90
91
92void GpsComponent::setPortCOM(const char * port)
93{
94 portName_ = port;
95}
96
97
98void GpsComponent::enableSocketServer(int /*portNumber*/)
99{
100
101}
102
103
104void GpsComponent::disableSocketServer()
105{
106
107}
108
109
110/************************************************************************/
111/* Main loop of the thread */
112/************************************************************************/
113void GpsComponent::run()
114{
115 int type = -1;
116
117 // parity:0-4=no,odd,even,mark,space
118 // byteSize:number of bits/byte, 4-8
119 // baudRate:port speed (ex:38400)
120 // stopBits:0,1,2 = 1, 1.5, 2
121
122 serialPort->configurePort(xmlParameters().getProperty("baudrate").toLong(),
123 xmlParameters().getProperty("bytesize").toUInt(),
124 // config.getProperty("parity").at(0).toLatin1(),
125 xmlParameters().getProperty("parity").toShort(),
126 xmlParameters().getProperty("stopbits").toUInt() - 1);
127
128
129 serialPort->start();
130
131 while (isActive())
132 {
133 // If there are no bytes remaining in the current frame, delete it
134 if ((currentFrame_ != NULL) && (nextByteToProcess_ >= currentFrame_->data.length())) {
135 delete currentFrame_;
136 currentFrame_ = NULL;
137 }
138
139 // If there is no current frame, get one. Wait for one if none is available.
140 if (currentFrame_ == NULL) {
141 semaphore_.acquire();
142 if ((currentFrame_ = serialPort->getNextFrame()) == NULL) {
143 // Should never get to this point as the semaphore counts the frames, unless
144 // the thread is being stopped.
145 continue;
146 } else {
147 nextByteToProcess_ = 0;
148 }
149 }
150
151 // Check if we got a PPS frame
152 if (currentFrameIsPps()) {
153 lastPpsTime_ = currentFrame_->t;
154 decodeFrame(SIGNAL_PPS);
155 } else if (analyzeFrame()) {
156 // a new complete NMEA frame has arrived, decode it.
157 type = frameType(frameToDecode_.data);
158 if (type != -1) {
159 if (decodeFrame(type) == -1) {
160 qWarning("Failed to decode the dataframe\n");
161 }
162 }
163
164 setState(ComponentBase::MONITOR_OK);
165 }
166 }
167
168 qDebug() << "The thread of " << name() << " GPS component is stopped";
169}
170
171
172
173/************************************************************************/
174/* Method to stop the thread */
175/************************************************************************/
176void GpsComponent::stopActivity()
177{
178 setActive( false );
179 unlockProcessing(1); // to unblock the thread that is waiting new data
180
181 serialPort->THREAD_ALIVE = false;
182
183 if (!serialPort->wait(2000))
184 {
185 serialPort->terminate();
186 qDebug("The Win32SerialPort thread blocks anormally, it has been killed !!");
187 }
188 if ( !serialPort->closePort() )
189 qDebug("Failed to close the port");
190 else
191 qDebug("The port is closed");
192 delete serialPort;
193
194 if (ppshdFile.isOpen())
195 ppshdFile.close();
196 if (ggahdFile.isOpen())
197 ggahdFile.close();
198 if (gsahdFile.isOpen())
199 gsahdFile.close();
200 if (gsthdFile.isOpen())
201 gsthdFile.close();
202 if (gsvhdFile.isOpen())
203 gsvhdFile.close();
204 if (hdthdFile.isOpen())
205 hdthdFile.close();
206 if (rmchdFile.isOpen())
207 rmchdFile.close();
208 if (rothdFile.isOpen())
209 rothdFile.close();
210 if (vtghdFile.isOpen())
211 vtghdFile.close();
212 if (zdahdFile.isOpen())
213 zdahdFile.close();
214}
215
216
217/************************************************************************/
218/* Method to start the thread */
219/************************************************************************/
220void GpsComponent::startActivity()
221{
222 ppsIndex_ = 0;
223
224 setActive( true );
225
226#if WIN32
227 serialPort = new Win32SerialPort(portName_.toLatin1());
228 // Asynchrone
229 serialPort->setMode(FILE_FLAG_OVERLAPPED);
230 // Synchrone
231 //serialPort->setMode(0);
232#else
233 serialPort = new PosixSerialPort(portName_.toLatin1());
234#endif
235
236 if (!serialPort->openPort(portName_.toLatin1()))
237 {
238 qDebug() << "Failed to open the port " << portName_;
239 qDebug() << "The GPS Component " << name() << " didn't start";
240 return;
241 }
242
243 serialPort->THREAD_ALIVE = true;
244 if (!QApplication::connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(unlockProcessing(int))))
245 qWarning("Failed to connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)\n" );
246 start();
247 qDebug() << "The Component " << name() << " is started";
248}
249
250
251/************************************************************************/
252/* To get the type of the received frame */
253/************************************************************************/
254
255// The NMEA frame code <-> pacpus type map
256GpsComponent::FrameTypeMap GpsComponent::frameTypes[GpsComponent::MAX_FRAMES] = {
257 { "PPS", SIGNAL_PPS },
258 { "$GPGGA", TRAME_GGA_DBL},
259 { "$GPGSA", TRAME_GSA },
260 { "$GPHDT", TRAME_HDT },
261 { "$GPGST", TRAME_GST },
262 { "$GPGSV", TRAME_GSV },
263 { "$GPRMC", TRAME_RMC },
264 { "$GPVTG", TRAME_VTG },
265 { "$GPZDA", TRAME_ZDA },
266};
267
268int GpsComponent::frameType(const QByteArray& frame)
269{
270 for (int i = 0; i < MAX_FRAMES; i++) {
271 if (frame.startsWith(frameTypes[i].code)) {
272 return frameTypes[i].type;
273 }
274 }
275
276 return UNKNOWN_NMEA_FRAME;
277}
278
279/************************************************************************/
280/* Returns true is the frame is a pps, without updating frameToDecode_ */
281/* which may already contain an in-band NMEA partial frame */
282/************************************************************************/
283bool GpsComponent::currentFrameIsPps() {
284 if (currentFrame_->data == "PPS") {
285 nextByteToProcess_ = 3;
286 return true;
287 } else {
288 return false;
289 }
290}
291
292/************************************************************************/
293/* Reconstruct the frame starting from the received fragments */
294/************************************************************************/
295bool GpsComponent::analyzeFrame()
296{
297 // Process the remaining bytes in the current frame
298 while(nextByteToProcess_ < currentFrame_->data.length()) {
299
300 char currentChar = currentFrame_->data[nextByteToProcess_++];
301
302 // first looking for start-of-frame
303 if (!startOfFrame_ && (currentChar == '$')) {
304 startOfFrame_ = true;
305 endOfFrame_ = false;
306
307 frameToDecode_.t = currentFrame_->t;
308 frameToDecode_.data.clear();
309 } else if (startOfFrame_ && !endOfFrame_ && (currentChar == '\n')) {
310 // Looking for end-of-frame
311 startOfFrame_ = false;
312 endOfFrame_ = true;
313 frameToDecode_.t = road_time();
314 //frameToDecode_.tr = static_cast<road_timerange_t>(currentFrame_->t - frameToDecode_.t);
315 return true; // There is a new frame to decode
316 }
317
318 if ((startOfFrame_) && (!endOfFrame_)) {
319 frameToDecode_.data.append(currentChar);
320 }
321 }
322 return false; // No new frame to decode, wait for more data
323}
324
325
326void GpsComponent::unlockProcessing(int v)
327{
328 // new frame available
329 semaphore_.release( v );
330}
331
332
333int GpsComponent::decodeFrame(int type)
334{
335 double lat_rad = 0, lon_rad = 0;
336 int indexGSV = 0;
337 int indexGSA = 0;
338
339
340 SENTENCE sentence;
341 sentence.Sentence = frameToDecode_.data;
342
343 switch(type)
344 {
345 case UNKNOWN_NMEA_FRAME:
346 qDebug("Unknown frame received !");
347 break;
348 case SIGNAL_PPS:
349 ppsFrame.setRoadTime(lastPpsTime_);
350 *ppsFrame.getFrameData() = ppsIndex_++;
351
352 if ( (isRecording()) && (ppsRecording) )
353 {
354 if (!(ppshdFile.isOpen()))
355 ppshdFile.open((char *)(name() + "_pps.dbt").toLatin1().data(),WriteMode,TYPE_INT,sizeof(int));
356 if ( ppshdFile.writeRecord(frameToDecode_.t,frameToDecode_.tr,(char *) ppsFrame.getFrameData(),sizeof(int)) == 0)
357 qWarning("Failed to record PPS data ...\n");
358 }
359 ppsFrame.notifyObservers();
360
361 break;
362
363 case TRAME_GGA_DBL:
364 if (!nmea0183_.Gga.Parse(sentence)) {
365 LOG_ERROR("Failed to parse the frame " << nmea0183_.Gga.ErrorMessage.toLatin1().data());
366 } else {
367 lat_rad = nmea0183_.Gga.Position.Latitude.GetDecimalDegrees()*PACPUS_PI/180;
368 lon_rad = nmea0183_.Gga.Position.Longitude.GetDecimalDegrees()*PACPUS_PI/180;
369 ggaFrame.getFrameData()->H = nmea0183_.Gga.Time.time().hour();
370 ggaFrame.getFrameData()->Mi = nmea0183_.Gga.Time.time().minute();
371 ggaFrame.getFrameData()->S = nmea0183_.Gga.Time.time().second();
372 ggaFrame.getFrameData()->Ms = nmea0183_.Gga.Time.time().msec();
373 ggaFrame.getFrameData()->lon = lon_rad;
374 ggaFrame.getFrameData()->lat = lat_rad;
375 ggaFrame.getFrameData()->ind_qualite = nmea0183_.Gga.GPSQuality;
376 ggaFrame.getFrameData()->nb_sat = nmea0183_.Gga.NumberOfSatellitesInUse;
377 ggaFrame.getFrameData()->hdop = nmea0183_.Gga.HorizontalDilutionOfPrecision;
378 ggaFrame.getFrameData()->alt_msl = nmea0183_.Gga.AntennaAltitudeMeters;
379 ggaFrame.getFrameData()->d_geoidal = nmea0183_.Gga.GeoidalSeparationMeters;
380 ggaFrame.getFrameData()->age = nmea0183_.Gga.AgeOfDifferentialGPSDataSeconds;
381 ggaFrame.getFrameData()->dir_lat = ( (nmea0183_.Gga.Position.Latitude.Northing == North) ? 'N' : 'S' );
382 ggaFrame.getFrameData()->dir_lon = ( (nmea0183_.Gga.Position.Longitude.Easting == East) ? 'E' : 'W' );
383 ggaFrame.getFrameData()->ref_station_ID = nmea0183_.Gga.DifferentialReferenceStationID;
384 ggaFrame.setRoadTime(frameToDecode_.t);
385
386 sendDataToServerSocket(*ggaFrame.getFrameData(),type);
387 ggaFrame.notifyObservers();
388
389 if ( (isRecording()) && (ggaRecording) ) {
390 if (!(ggahdFile.isOpen()))
391 ggahdFile.open((char *)(name() + "_gga.dbt").toLatin1().data(),WriteMode,TRAME_GGA_DBL,sizeof(trame_gga_dbl));
392 if ( !ggahdFile.writeRecord(frameToDecode_.t,frameToDecode_.tr,(char *) ggaFrame.getFrameData(),sizeof(trame_gga_dbl)))
393 qWarning("Failed to record GGA data ...\n");
394 }
395 //printf("lon=%f lat=%f\n",ggaFrame.getFrameData()->lon, ggaFrame.getFrameData()->lat);
396 }
397 break;
398
399 case TRAME_GSA:
400 if (!nmea0183_.Gsa.Parse(sentence))
401 qWarning("Failed to parse the frame %s\n", nmea0183_.Gsa.ErrorMessage.toLatin1().data());
402 gsaFrame.getFrameData()->mode_select = ((nmea0183_.Gsa.OperatingMode == GSA::Manual) ? 'M' : 'A');
403 gsaFrame.getFrameData()->mode_result = 0;
404 if (nmea0183_.Gsa.FixMode == GSA::FixUnavailable)
405 gsaFrame.getFrameData()->mode_result = 1;
406 if (nmea0183_.Gsa.FixMode == GSA::TwoDimensional)
407 gsaFrame.getFrameData()->mode_result = 2;
408 if (nmea0183_.Gsa.FixMode == GSA::ThreeDimensional)
409 gsaFrame.getFrameData()->mode_result = 3;
410 for (indexGSA = 0 ; indexGSA<12 ; indexGSA++)
411 gsaFrame.getFrameData()->SV_PRN[indexGSA] = nmea0183_.Gsa.SatelliteNumber[indexGSA];
412 gsaFrame.getFrameData()->pdop = nmea0183_.Gsa.PDOP;
413 gsaFrame.getFrameData()->hdop = nmea0183_.Gsa.HDOP;
414 gsaFrame.getFrameData()->vdop = nmea0183_.Gsa.VDOP;
415 gsaFrame.setRoadTime(frameToDecode_.t);
416
417 gsaFrame.notifyObservers();
418
419 sendDataToServerSocket(*gsaFrame.getFrameData(),type);
420
421 if ( (isRecording()) && (gsaRecording) )
422 {
423 if (!(gsahdFile.isOpen()))
424 gsahdFile.open((char *)(name() + "_gsa.dbt").toLatin1().data(),WriteMode, TRAME_GSA,sizeof(trame_gsa));
425 if ( gsahdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) gsaFrame.getFrameData(),sizeof(trame_gsa)) != 1)
426 qWarning("Failed to record GSA data ...\n");
427 }
428 break;
429
430
431
432 case TRAME_GST:
433 if (!nmea0183_.Gst.Parse( sentence ))
434 qWarning("Failed to parse the frame %s\n",nmea0183_.Gst.ErrorMessage.toLatin1().data());
435 gstFrame.getFrameData()->rms = nmea0183_.Gst.RMSvalue;
436 gstFrame.getFrameData()->a = nmea0183_.Gst.ErrorEllipseMajor;
437 gstFrame.getFrameData()->b = nmea0183_.Gst.ErrorEllipseMinor;
438 gstFrame.getFrameData()->phi = nmea0183_.Gst.ErrorEllipseOrientation;
439 gstFrame.getFrameData()->sigma_lat = nmea0183_.Gst.LatitudeError;
440 gstFrame.getFrameData()->sigma_lon = nmea0183_.Gst.LongitudeError;
441 gstFrame.getFrameData()->sigma_alt = nmea0183_.Gst.HeightError;
442 gstFrame.getFrameData()->H = nmea0183_.Gst.Time.time().hour();
443 gstFrame.getFrameData()->Mi = nmea0183_.Gst.Time.time().minute();
444 gstFrame.getFrameData()->S = nmea0183_.Gst.Time.time().second();
445 gstFrame.getFrameData()->Ms = nmea0183_.Gst.Time.time().msec();
446 gstFrame.setRoadTime(frameToDecode_.t);
447
448 sendDataToServerSocket(*gstFrame.getFrameData(),type);
449 gstFrame.notifyObservers();
450
451
452 if ( (isRecording()) && (gstRecording) ) {
453 if (!gsthdFile.isOpen())
454 gsthdFile.open((char *)(name() + "_gst.dbt").toLatin1().data(),WriteMode,TRAME_GST,sizeof(trame_gst));
455 if ( !gsthdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) gstFrame.getFrameData(),sizeof(trame_gst)) )
456 qWarning("Failed to record GST data ...\n");
457 }
458 break;
459
460 case TRAME_GSV:
461 indexGSV = 0;
462 if (!nmea0183_.Gsv.Parse( sentence )) {
463 qWarning("Failed to parse the frame %s\n",nmea0183_.Gsv.ErrorMessage.toLatin1().data());
464 break;
465 }
466 // it's a new frame, reset stored value in case of the number of satellites
467 // in view has decreased
468 if (nmea0183_.Gsv.message_number == 1)
469 {
470 while (indexGSV < 36)
471 {
472 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 0 ] = 0;
473 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 1 ] = 0;
474 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 2 ] = 0;
475 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 3 ] = 0;
476 indexGSV++;
477 }
478 }
479 gsvFrame.getFrameData()->NumberOfSatellites = nmea0183_.Gsv.NumberOfSatellites;
480 gsvFrame.getFrameData()->Totalmessages = nmea0183_.Gsv.Totalmessages;
481
482 for ( indexGSV=4*(nmea0183_.Gsv.message_number-1); indexGSV<=(4*nmea0183_.Gsv.message_number)-1; indexGSV++ )
483 {
484 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 0 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SatelliteNumber;
485 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 1 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].ElevationDegrees;
486 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 2 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].AzimuthDegreesTrue;
487 gsvFrame.getFrameData()->SatellitesInView[ indexGSV ][ 3 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SignalToNoiseRatio;
488 }
489
490 if (nmea0183_.Gsv.Totalmessages == nmea0183_.Gsv.message_number) {
491 sendDataToServerSocket(*gsvFrame.getFrameData(),type);
492 gsvFrame.setRoadTime(frameToDecode_.t);
493 gsvFrame.notifyObservers();
494 }
495 if ( (isRecording()) && (gsvRecording) && (nmea0183_.Gsv.Totalmessages == nmea0183_.Gsv.message_number) )
496 {
497 if (!gsvhdFile.isOpen())
498 gsvhdFile.open((char *)(name() + "_gsv.dbt").toLatin1().data(),WriteMode,TRAME_GSV,sizeof(trame_gsv));
499 if ( gsvhdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) gsvFrame.getFrameData(),sizeof(trame_gsv)) == 0)
500 qWarning("Failed to record GSV data ...\n");
501 }
502 break;
503
504 case TRAME_HDT:
505 if (!nmea0183_.Hdt.Parse( sentence ))
506 qWarning("Failed to parse the frame %s\n",nmea0183_.Hdt.ErrorMessage.toLatin1().data());
507 hdtFrame.getFrameData()->DegreesTrue = nmea0183_.Hdt.DegreesTrue;
508 hdtFrame.setRoadTime(frameToDecode_.t);
509
510 sendDataToServerSocket(hdtFrame,type);
511 hdtFrame.notifyObservers();
512
513 if ( (isRecording()) && (hdtRecording) )
514 {
515 if (!hdthdFile.isOpen())
516 hdthdFile.open((char *)(name() + "_hdt.dbt").toLatin1().data(),WriteMode,TRAME_HDT,sizeof(trame_hdt));
517 if ( hdthdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) hdtFrame.getFrameData(),sizeof(trame_hdt)) == 0)
518 qWarning("Failed to record HDT data ...\n");
519 }
520 break;
521
522 case TRAME_RMC:
523 if (!nmea0183_.Rmc.Parse( sentence ))
524 qWarning("Failed to parse the frame %s\n",nmea0183_.Rmc.ErrorMessage.toLatin1().data());
525 rmcFrame.getFrameData()->H = nmea0183_.Rmc.Time.time().hour();
526 rmcFrame.getFrameData()->Mi = nmea0183_.Rmc.Time.time().minute();
527 rmcFrame.getFrameData()->S = nmea0183_.Rmc.Time.time().second();
528 rmcFrame.getFrameData()->Ms = nmea0183_.Rmc.Time.time().msec();
529 rmcFrame.getFrameData()->AA = nmea0183_.Rmc.Time.date().year();
530 rmcFrame.getFrameData()->MM = nmea0183_.Rmc.Time.date().month();
531 rmcFrame.getFrameData()->JJ = nmea0183_.Rmc.Time.date().day();
532 rmcFrame.getFrameData()->lat = nmea0183_.Rmc.Position.Latitude.GetDecimalDegrees()*PACPUS_PI/180;
533 rmcFrame.getFrameData()->dir_lat = ( (nmea0183_.Rmc.Position.Latitude.Northing == North) ? 'N' : 'S');
534 rmcFrame.getFrameData()->lon = nmea0183_.Rmc.Position.Longitude.GetDecimalDegrees()*PACPUS_PI/180;;
535 rmcFrame.getFrameData()->dir_lon = ( (nmea0183_.Rmc.Position.Longitude.Easting == East) ? 'E' : 'W' );
536 rmcFrame.getFrameData()->magnet_var = nmea0183_.Rmc.MagneticVariation;
537 rmcFrame.getFrameData()->dir_magnet_var = ( (nmea0183_.Rmc.MagneticVariationDirection == East) ? 'E' : 'W');
538 rmcFrame.getFrameData()->mode = -1;
539 if (nmea0183_.Rmc.ModeIndication == "A")
540 rmcFrame.getFrameData()->mode = 1;
541 if (nmea0183_.Rmc.ModeIndication == "D")
542 rmcFrame.getFrameData()->mode = 2;
543 if (nmea0183_.Rmc.ModeIndication == "N")
544 rmcFrame.getFrameData()->mode = 0;
545 rmcFrame.getFrameData()->track_true_north = nmea0183_.Rmc.TrackMadeGoodDegreesTrue;
546 rmcFrame.getFrameData()->valid_data = ( (nmea0183_.Rmc.IsDataValid == True) ? 1 : 0 );
547 rmcFrame.getFrameData()->vitesse = nmea0183_.Rmc.SpeedOverGroundKnots * 1852.0 / 3600.0; // 1 knot = 1852 m/h
548 rmcFrame.setRoadTime(frameToDecode_.t);
549
550 sendDataToServerSocket(*rmcFrame.getFrameData(),type);
551 rmcFrame.notifyObservers();
552
553 if ( (isRecording()) && (rmcRecording) )
554 {
555 if (!rmchdFile.isOpen())
556 rmchdFile.open((char *)(name() + "_rmc.dbt").toLatin1().data(),WriteMode,TRAME_RMC,sizeof(trame_rmc));
557 if (rmchdFile.writeRecord(frameToDecode_.t ,frameToDecode_.tr,(char *) rmcFrame.getFrameData(),sizeof(trame_rmc)) == 0)
558 qWarning("Failed to record RMC data ...\n");
559 }
560 break;
561
562 case TRAME_ROT:
563 if (!nmea0183_.Rot.Parse( sentence ))
564 qWarning("Failed to parse the frame %s\n",nmea0183_.Rot.ErrorMessage.toLatin1().data());
565 rotFrame.getFrameData()->RateOfTurn = nmea0183_.Rot.RateOfTurn;
566 rotFrame.getFrameData()->valid_data = ( (nmea0183_.Rot.IsDataValid == True) ? 1 : 0 );
567 rotFrame.setRoadTime(frameToDecode_.t);
568
569 sendDataToServerSocket(*rotFrame.getFrameData(),type);
570 rotFrame.notifyObservers();
571
572 if ( (isRecording()) && (rotRecording) )
573 {
574 if (!rothdFile.isOpen())
575 rothdFile.open((char *)(name() + "_rot.dbt").toLatin1().data(),WriteMode,TRAME_ROT,sizeof(trame_rot));
576 if ( rothdFile.writeRecord(frameToDecode_.t ,frameToDecode_.tr,(char *) rotFrame.getFrameData(),sizeof(trame_rot)) == 0)
577 qWarning("Failed to record ROT data ...\n");
578 }
579 break;
580
581 case TRAME_VTG:
582 if (!nmea0183_.Vtg.Parse( sentence )) {
583 LOG_WARN("Failed to parse the frame " << nmea0183_.Vtg.ErrorMessage);
584 } else {
585 vtgFrame.getFrameData()->v = nmea0183_.Vtg.SpeedKilometersPerHour;
586 vtgFrame.getFrameData()->track_true_north = nmea0183_.Vtg.TrackDegreesTrue;
587 vtgFrame.getFrameData()->track_magnet_north = nmea0183_.Vtg.TrackDegreesMagnetic;
588 vtgFrame.setRoadTime(frameToDecode_.t);
589
590 sendDataToServerSocket(vtgFrame,type);
591 vtgFrame.notifyObservers();
592
593 if (isRecording() && vtgRecording) {
594 if (!vtghdFile.isOpen())
595 vtghdFile.open((char *)(name() + "_vtg.dbt").toLatin1().data(),WriteMode,TRAME_VTG,sizeof(trame_vtg));
596 if ( vtghdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) vtgFrame.getFrameData(),sizeof(trame_vtg)) == 0)
597 qWarning("Failed to record VTG data ...\n");
598 }
599 }
600 break;
601
602 case TRAME_ZDA:
603 if (!nmea0183_.Zda.Parse( sentence )) {
604 LOG_WARN("Failed to parse the frame " << nmea0183_.Zda.ErrorMessage);
605 }
606 zdaFrame.getFrameData()->H = nmea0183_.Zda.Time.time().hour();
607 zdaFrame.getFrameData()->Mi = nmea0183_.Zda.Time.time().minute();
608 zdaFrame.getFrameData()->S = nmea0183_.Zda.Time.time().second();
609 zdaFrame.getFrameData()->Ms = nmea0183_.Zda.Time.time().msec();
610 zdaFrame.getFrameData()->AA = nmea0183_.Zda.Time.date().year();
611 zdaFrame.getFrameData()->MM = nmea0183_.Zda.Time.date().month();
612 zdaFrame.getFrameData()->JJ = nmea0183_.Zda.Time.date().day();
613 zdaFrame.getFrameData()->H_offset = nmea0183_.Zda.LocalHourDeviation;
614 zdaFrame.getFrameData()->Mi_offset = nmea0183_.Zda.LocalMinutesDeviation;
615
616 sendDataToServerSocket(*zdaFrame.getFrameData(), type);
617 zdaFrame.setRoadTime(frameToDecode_.t);
618 zdaFrame.notifyObservers();
619
620 if ( (isRecording()) && (zdaRecording) ) {
621 if (!zdahdFile.isOpen())
622 zdahdFile.open((char *)(name() + "_zda.dbt").toLatin1().data(),WriteMode,TRAME_ZDA,sizeof(trame_zda));
623 if ( zdahdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) zdaFrame.getFrameData(),sizeof(trame_zda)) == 0) {
624 LOG_WARN("Failed to record ZDA data ...");
625 }
626 }
627
628 /*
629 if (isRecording() && zdaRecording && ppsRecording) {
630 memcpy(&(synchroFrame.getFrameData()->zda), zdaFrame.getFrameData(), sizeof(zdaFrame));
631 synchroFrame.getFrameData()->timeOffset = frameToDecode_.t - lastPpsTime_;
632 synchroFrame.getFrameData()->ppsTime = lastPpsTime_;
633 synchroFrame.getFrameData()->zdaTime = frameToDecode_.t;
634 synchroFrame.setRoadTime(frameToDecode_.t);
635
636 if (!gpsSynchroFile.isOpen()) {
637 gpsSynchroFile.open((name() + "_synchro.dbt").toStdString(), WriteMode, GPS_SYNCHRO_FRAME, sizeof(GpsSynchroFrame) );
638 }
639
640 if (gpsSynchroFile.writeRecord(frameToDecode_.t, frameToDecode_.t - lastPpsTime_, reinterpret_cast<const char *>(synchroFrame.getFrameData()), sizeof(GpsSynchroFrame)) == 0) {
641 LOG_WARN("Failed to record GPS SYNCHRO data ...");
642 }
643 }
644 */
645
646 break;
647
648 default:
649 return 0;
650 }
651
652 return 1;
653}
654
655} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.