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

Last change on this file since 119 was 119, checked in by nguyenhu, 8 years ago

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