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

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

compilation under linux with 0.2.X framework

File size: 24.2 KB
RevLine 
[59]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>
[99]18#include <iostream>
[59]19
[99]20#define PACPUS_PI 3.1415926
21
[59]22namespace pacpus {
23
24/// Construct the factory
[99]25ComponentFactory<GpsComponent> sFactory("GpsComponent");
[59]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
[99]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;
[59]63*/
[99]64
65
[59]66 nextByteToProcess_ = 0;
67 currentFrame_ = NULL;
[99]68 newFrameToDecode_ = false;
69 startOfFrame_ = false;
[59]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{
[99]85 setPortCOM( config.getProperty("port").toLatin1() );
[72]86 setRecording ((config.getProperty("recording") == "true" ? true : false));
[59]87
88 return ComponentBase::CONFIGURED_OK;
89}
90
91
92void GpsComponent::setPortCOM(const char * port)
93{
[99]94 portName_ = port;
[59]95}
96
97
98void GpsComponent::enableSocketServer(int /*portNumber*/)
99{
[77]100
[59]101}
102
103
104void GpsComponent::disableSocketServer()
105{
[77]106
[59]107}
108
109
110/************************************************************************/
111/* Main loop of the thread */
112/************************************************************************/
113void GpsComponent::run()
114{
[99]115 int type = -1;
116
[59]117 // parity:0-4=no,odd,even,mark,space
118 // byteSize:number of bits/byte, 4-8
[99]119 // baudRate:port speed (ex:38400)
120 // stopBits:0,1,2 = 1, 1.5, 2
121
122 serialPort->configurePort(xmlParameters().getProperty("baudrate").toLong(),
[59]123 xmlParameters().getProperty("bytesize").toUInt(),
124 // config.getProperty("parity").at(0).toLatin1(),
125 xmlParameters().getProperty("parity").toShort(),
[99]126 xmlParameters().getProperty("stopbits").toUInt() - 1);
[59]127
128
[99]129 serialPort->start();
130
[59]131 while (isActive())
[99]132 {
[59]133 // If there are no bytes remaining in the current frame, delete it
134 if ((currentFrame_ != NULL) && (nextByteToProcess_ >= currentFrame_->data.length())) {
[99]135 delete currentFrame_;
[59]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) {
[99]143 // Should never get to this point as the semaphore counts the frames, unless
[59]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);
[99]155 } else if (analyzeFrame()) {
[59]156 // a new complete NMEA frame has arrived, decode it.
[99]157 type = frameType(frameToDecode_.data);
[59]158 if (type != -1) {
159 if (decodeFrame(type) == -1) {
[99]160 qWarning("Failed to decode the dataframe\n");
[59]161 }
162 }
163
164 setState(ComponentBase::MONITOR_OK);
165 }
166 }
167
[99]168 qDebug() << "The thread of " << name() << " GPS component is stopped";
[59]169}
170
171
172
173/************************************************************************/
174/* Method to stop the thread */
175/************************************************************************/
176void GpsComponent::stopActivity()
177{
[99]178 setActive( false );
[59]179 unlockProcessing(1); // to unblock the thread that is waiting new data
180
[99]181 serialPort->THREAD_ALIVE = false;
182
[59]183 if (!serialPort->wait(2000))
184 {
[99]185 serialPort->terminate();
[59]186 qDebug("The Win32SerialPort thread blocks anormally, it has been killed !!");
187 }
[99]188 if ( !serialPort->closePort() )
189 qDebug("Failed to close the port");
[59]190 else
[99]191 qDebug("The port is closed");
[59]192 delete serialPort;
193
194 if (ppshdFile.isOpen())
[99]195 ppshdFile.close();
[59]196 if (ggahdFile.isOpen())
197 ggahdFile.close();
198 if (gsahdFile.isOpen())
[99]199 gsahdFile.close();
[59]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())
[99]207 rmchdFile.close();
[59]208 if (rothdFile.isOpen())
209 rothdFile.close();
210 if (vtghdFile.isOpen())
[99]211 vtghdFile.close();
[59]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;
[99]223
224 setActive( true );
225
[59]226#if WIN32
[99]227 serialPort = new Win32SerialPort(portName_.toLatin1());
[59]228 // Asynchrone
[99]229 serialPort->setMode(FILE_FLAG_OVERLAPPED);
[59]230 // Synchrone
[99]231 //serialPort->setMode(0);
232#else
233 serialPort = new PosixSerialPort(portName_.toLatin1());
[59]234#endif
[99]235
[59]236 if (!serialPort->openPort(portName_.toLatin1()))
237 {
238 qDebug() << "Failed to open the port " << portName_;
239 qDebug() << "The GPS Component " << name() << " didn't start";
[99]240 return;
[59]241 }
[99]242
243 serialPort->THREAD_ALIVE = true;
[59]244 if (!QApplication::connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(unlockProcessing(int))))
[99]245 qWarning("Failed to connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)\n" );
246 start();
[59]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
[99]303 if (!startOfFrame_ && (currentChar == '$')) {
[59]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 }
[99]317
[59]318 if ((startOfFrame_) && (!endOfFrame_)) {
[99]319 frameToDecode_.data.append(currentChar);
[59]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
[99]329 semaphore_.release( v );
[59]330}
331
332
333int GpsComponent::decodeFrame(int type)
334{
[99]335 double lat_rad = 0, lon_rad = 0;
336 int indexGSV = 0;
[59]337 int indexGSA = 0;
[99]338
339
340 SENTENCE sentence;
341 sentence.Sentence = frameToDecode_.data;
342
343 switch(type)
[59]344 {
[99]345 case UNKNOWN_NMEA_FRAME:
346 qDebug("Unknown frame received !");
347 break;
[59]348 case SIGNAL_PPS:
349 ppsFrame.setRoadTime(lastPpsTime_);
350 *ppsFrame.getFrameData() = ppsIndex_++;
351
352 if ( (isRecording()) && (ppsRecording) )
353 {
354 if (!(ppshdFile.isOpen()))
[99]355 ppshdFile.open((char *)(name() + "_pps.dbt").toLatin1().data(),WriteMode,TYPE_INT,sizeof(int));
[59]356 if ( ppshdFile.writeRecord(frameToDecode_.t,frameToDecode_.tr,(char *) ppsFrame.getFrameData(),sizeof(int)) == 0)
[99]357 qWarning("Failed to record PPS data ...\n");
[59]358 }
359 ppsFrame.notifyObservers();
360
361 break;
[99]362
363 case TRAME_GGA_DBL:
[59]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;
[99]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;
[59]384 ggaFrame.setRoadTime(frameToDecode_.t);
[99]385
386 sendDataToServerSocket(*ggaFrame.getFrameData(),type);
[59]387 ggaFrame.notifyObservers();
388
389 if ( (isRecording()) && (ggaRecording) ) {
390 if (!(ggahdFile.isOpen()))
[99]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");
[59]394 }
395 //printf("lon=%f lat=%f\n",ggaFrame.getFrameData()->lon, ggaFrame.getFrameData()->lat);
396 }
397 break;
[99]398
[59]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)
[99]405 gsaFrame.getFrameData()->mode_result = 1;
[59]406 if (nmea0183_.Gsa.FixMode == GSA::TwoDimensional)
[99]407 gsaFrame.getFrameData()->mode_result = 2;
[59]408 if (nmea0183_.Gsa.FixMode == GSA::ThreeDimensional)
[99]409 gsaFrame.getFrameData()->mode_result = 3;
[59]410 for (indexGSA = 0 ; indexGSA<12 ; indexGSA++)
[99]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;
[59]415 gsaFrame.setRoadTime(frameToDecode_.t);
416
417 gsaFrame.notifyObservers();
[99]418
419 sendDataToServerSocket(*gsaFrame.getFrameData(),type);
420
[59]421 if ( (isRecording()) && (gsaRecording) )
422 {
423 if (!(gsahdFile.isOpen()))
424 gsahdFile.open((char *)(name() + "_gsa.dbt").toLatin1().data(),WriteMode, TRAME_GSA,sizeof(trame_gsa));
[99]425 if ( gsahdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) gsaFrame.getFrameData(),sizeof(trame_gsa)) != 1)
426 qWarning("Failed to record GSA data ...\n");
[59]427 }
428 break;
429
430
[99]431
[59]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);
[99]447
448 sendDataToServerSocket(*gstFrame.getFrameData(),type);
[59]449 gstFrame.notifyObservers();
450
451
452 if ( (isRecording()) && (gstRecording) ) {
[99]453 if (!gsthdFile.isOpen())
454 gsthdFile.open((char *)(name() + "_gst.dbt").toLatin1().data(),WriteMode,TRAME_GST,sizeof(trame_gst));
[59]455 if ( !gsthdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) gstFrame.getFrameData(),sizeof(trame_gst)) )
[99]456 qWarning("Failed to record GST data ...\n");
[59]457 }
[99]458 break;
459
[59]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 }
[99]466 // it's a new frame, reset stored value in case of the number of satellites
[59]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;
[99]481
[59]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) {
[99]491 sendDataToServerSocket(*gsvFrame.getFrameData(),type);
[59]492 gsvFrame.setRoadTime(frameToDecode_.t);
493 gsvFrame.notifyObservers();
494 }
495 if ( (isRecording()) && (gsvRecording) && (nmea0183_.Gsv.Totalmessages == nmea0183_.Gsv.message_number) )
496 {
[99]497 if (!gsvhdFile.isOpen())
498 gsvhdFile.open((char *)(name() + "_gsv.dbt").toLatin1().data(),WriteMode,TRAME_GSV,sizeof(trame_gsv));
[59]499 if ( gsvhdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) gsvFrame.getFrameData(),sizeof(trame_gsv)) == 0)
[99]500 qWarning("Failed to record GSV data ...\n");
[59]501 }
[99]502 break;
503
[59]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);
[99]509
510 sendDataToServerSocket(hdtFrame,type);
[59]511 hdtFrame.notifyObservers();
[99]512
[59]513 if ( (isRecording()) && (hdtRecording) )
514 {
515 if (!hdthdFile.isOpen())
[99]516 hdthdFile.open((char *)(name() + "_hdt.dbt").toLatin1().data(),WriteMode,TRAME_HDT,sizeof(trame_hdt));
[59]517 if ( hdthdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) hdtFrame.getFrameData(),sizeof(trame_hdt)) == 0)
[99]518 qWarning("Failed to record HDT data ...\n");
[59]519 }
[99]520 break;
521
[59]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);
[99]549
550 sendDataToServerSocket(*rmcFrame.getFrameData(),type);
[59]551 rmcFrame.notifyObservers();
[99]552
[59]553 if ( (isRecording()) && (rmcRecording) )
554 {
555 if (!rmchdFile.isOpen())
[99]556 rmchdFile.open((char *)(name() + "_rmc.dbt").toLatin1().data(),WriteMode,TRAME_RMC,sizeof(trame_rmc));
[59]557 if (rmchdFile.writeRecord(frameToDecode_.t ,frameToDecode_.tr,(char *) rmcFrame.getFrameData(),sizeof(trame_rmc)) == 0)
[99]558 qWarning("Failed to record RMC data ...\n");
[59]559 }
560 break;
[99]561
[59]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);
[99]568
569 sendDataToServerSocket(*rotFrame.getFrameData(),type);
[59]570 rotFrame.notifyObservers();
[99]571
[59]572 if ( (isRecording()) && (rotRecording) )
573 {
574 if (!rothdFile.isOpen())
[99]575 rothdFile.open((char *)(name() + "_rot.dbt").toLatin1().data(),WriteMode,TRAME_ROT,sizeof(trame_rot));
[59]576 if ( rothdFile.writeRecord(frameToDecode_.t ,frameToDecode_.tr,(char *) rotFrame.getFrameData(),sizeof(trame_rot)) == 0)
[99]577 qWarning("Failed to record ROT data ...\n");
[59]578 }
[99]579 break;
580
[59]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;
[99]586 vtgFrame.getFrameData()->track_true_north = nmea0183_.Vtg.TrackDegreesTrue;
587 vtgFrame.getFrameData()->track_magnet_north = nmea0183_.Vtg.TrackDegreesMagnetic;
[59]588 vtgFrame.setRoadTime(frameToDecode_.t);
[99]589
[59]590 sendDataToServerSocket(vtgFrame,type);
591 vtgFrame.notifyObservers();
[99]592
[59]593 if (isRecording() && vtgRecording) {
[99]594 if (!vtghdFile.isOpen())
595 vtghdFile.open((char *)(name() + "_vtg.dbt").toLatin1().data(),WriteMode,TRAME_VTG,sizeof(trame_vtg));
[59]596 if ( vtghdFile.writeRecord(frameToDecode_.t, frameToDecode_.tr,(char *) vtgFrame.getFrameData(),sizeof(trame_vtg)) == 0)
[99]597 qWarning("Failed to record VTG data ...\n");
[59]598 }
599 }
[99]600 break;
601
[59]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();
[99]613 zdaFrame.getFrameData()->H_offset = nmea0183_.Zda.LocalHourDeviation;
614 zdaFrame.getFrameData()->Mi_offset = nmea0183_.Zda.LocalMinutesDeviation;
615
[59]616 sendDataToServerSocket(*zdaFrame.getFrameData(), type);
617 zdaFrame.setRoadTime(frameToDecode_.t);
618 zdaFrame.notifyObservers();
[99]619
[59]620 if ( (isRecording()) && (zdaRecording) ) {
[99]621 if (!zdahdFile.isOpen())
622 zdahdFile.open((char *)(name() + "_zda.dbt").toLatin1().data(),WriteMode,TRAME_ZDA,sizeof(trame_zda));
[59]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 */
[99]645
646 break;
647
[59]648 default:
[99]649 return 0;
[59]650 }
[99]651
652 return 1;
[59]653}
654
655} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.