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

Last change on this file since 69 was 59, checked in by DHERBOMEZ Gérald, 11 years ago

Integration of new modules:

  • GPS NMEA0183 decoder
  • Span CPT Decoder

Update of:

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