source: pacpusframework/branches/2.0-beta1/src/TestComponents/CPT/sensor/CPTComponent.cpp@ 89

Last change on this file since 89 was 89, checked in by morasjul, 11 years ago

PACPUS 2.0 Beta deployed in new branch

Major changes:
-Add communication interface between components
-Add examples for communications interface (TestComponents)
-Move to Qt5 support

  • Property svn:executable set to *
File size: 43.4 KB
Line 
1// *********************************************************************
2// created: 20011/06/28 - 18:13
3// filename: cptComponent.cpp
4//
5// author: Sergio Rodriguez
6//
7// version: $Id: $
8//
9// purpose: Acquires SPAN CPT data
10//
11// *********************************************************************
12
13#include "CPTComponent.h"
14
15#include <iostream>
16
17#include "kernel/ComponentFactory.h"
18#include "kernel/DbiteFileTypes.h"
19#include "kernel/Log.h"
20#include "PacpusTools/geodesie.h"
21#include "PacpusTools/ShMem.h"
22
23using namespace pacpus;
24using namespace std;
25
26/// Construct the factory
27static ComponentFactory<CPTComponent> sFactory("CPTComponent");
28
29static const size_t AllFramesSize = sizeof(TimestampedBestgpsposaFrame)
30+ sizeof(TimestampedRawimusaFrame)
31+ sizeof(TimestampedInspvaaFrame)
32+ sizeof(TimestampedInscovFrame)
33;
34
35DECLARE_STATIC_LOGGER("pacpus.base.CPTComponent");
36
37void CPTComponent::addInputOutput()
38{
39 ADD_OUTPUT("bestposegpsposa",CPTComponent,TimestampedBestgpsposaFrame);
40 ADD_OUTPUT("rawimusa",CPTComponent,TimestampedRawimusaFrame);
41 ADD_OUTPUT("inspvaaa",CPTComponent,TimestampedInspvaaFrame);
42 ADD_OUTPUT("inscov",CPTComponent,TimestampedInscovFrame);
43 ADD_OUTPUT("genericpose",CPTComponent,Pose3D);
44}
45
46
47//////////////////////////////////////////////////////////////////////////
48/// Constructor of CPTComponent class
49CPTComponent::CPTComponent(QString name)
50 : ComponentBase(name)
51{
52 LOG_INFO("sizeof(TimestampedBestgpsposaFrame)=" << sizeof(TimestampedBestgpsposaFrame));
53 LOG_INFO("sizeof(TimestampedRawimusaFrame)=" << sizeof(TimestampedRawimusaFrame));
54 LOG_INFO("sizeof(TimestampedInspvaaFrame)=" << sizeof(TimestampedInspvaaFrame));
55 LOG_INFO("sizeof(TimestampedInscovFrame)=" << sizeof(TimestampedInscovFrame));
56
57 mVerbose = false;
58 recording = false;
59 enuRef_ = false;
60 LAT_REF =0.0;
61 LON_REF =0.0;
62 HE_REF =0.0;
63
64 currentRoadtime_ = 0;
65 currentTimerange_ = 0;
66
67 //decoding
68 currentDataFrame ="";
69 currentRoadtime = 0;
70 currentTimerange = 0;
71
72 frameToDecode = "";
73 timeOfFrameToDecode = 0;
74 timerangeOfFrameToDecode = 0;
75
76 restOfFrame = "";
77
78 newFrameToDecode = false;
79 startOfFrame = false;
80 sofIdx_ =0;
81 endOfFrame = false;
82 eofIdx_ =0;
83
84 //ShMems allocation
85 mShMem = new ShMem("SPAN_FRAMES", AllFramesSize);
86 mAllFramesBuffer = new char[AllFramesSize];
87 addInputOutput();
88}
89
90/// Destructor of the CPTComponent class
91CPTComponent::~CPTComponent()
92{
93 delete mShMem;
94 delete[] mAllFramesBuffer;
95}
96
97ComponentBase::COMPONENT_CONFIGURATION CPTComponent::configureComponent(XmlComponentConfig /*config*/)
98{
99 setPortCOM( param.getProperty("port").toLatin1() );
100
101 if (!param.getProperty("recording").isNull()) {
102 recording = (param.getProperty("recording") == "true" ? true : false);
103 }
104// if (!param.getProperty("verbose").isNull()) {
105// mVerbose = (param.getProperty("verbose") == "true" ? true : false);
106// }
107 if (!param.getProperty("ENUref").isNull()) {
108 enuRef_ = (param.getProperty("ENUref") == "true" ? true : false);
109 if (enuRef_) {
110 LAT_REF = param.getProperty("LatRef").toDouble();
111 LON_REF = param.getProperty("LonRef").toDouble();
112 HE_REF = param.getProperty("HeRef").toDouble();
113 LOG_DEBUG("Reference Saint Mande:: Lat: " << LAT_REF << ", Lon: " << LON_REF << " Height: " << HE_REF);
114 }
115 }
116 return ComponentBase::CONFIGURED_OK;
117}
118
119void CPTComponent::setPortCOM(const char * port)
120{
121 portName = port;
122}
123
124/// Log function
125void CPTComponent::processing(int /*i*/)
126{
127 if (!serialPort) {
128 LOG_WARN("no serial port");
129 return;
130 }
131
132 //ShMem data variables
133 TimestampedBestgpsposaFrame a;
134 TimestampedRawimusaFrame b;
135 TimestampedInspvaaFrame c;
136 TimestampedInscovFrame d;
137
138 //ShMem buffer allocation
139 char * allFramesBuffer = new char[AllFramesSize];
140
141 //ShMem buffer initialization
142 initBuffer(&a, &b, &c, &d);
143 memcpy(allFramesBuffer,
144 &a, sizeof(TimestampedBestgpsposaFrame));
145 memcpy(allFramesBuffer
146 + sizeof(TimestampedBestgpsposaFrame),
147 &b,sizeof(TimestampedRawimusaFrame));
148 memcpy(allFramesBuffer
149 + sizeof(TimestampedBestgpsposaFrame) + sizeof(TimestampedRawimusaFrame),
150 &c, sizeof(TimestampedInspvaaFrame));
151 memcpy(allFramesBuffer
152 + sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame)+sizeof(TimestampedInspvaaFrame),
153 &d, sizeof(TimestampedInscovFrame));
154
155 memcpy(allFramesBuffer, mAllFramesBuffer, AllFramesSize);
156
157 // get the frame and remove it in the list
158 FRAME * currentFrame = serialPort->firstFrame();
159
160 char * currentDataFrame = new char[currentFrame->length];
161 memcpy(currentDataFrame, currentFrame->data, currentFrame->length);
162 currentDataFrameLength_ = currentFrame->length;
163 currentRoadtime_ = currentFrame->t;
164
165 serialPort->removeFirstFrame();
166
167 setState(ComponentBase::MONITOR_OK);
168
169 if (recording) {
170 s8Data_.dataPos = dataFile_.tellp();
171 s8Data_.length = currentDataFrameLength_;
172
173 LOG_DEBUG("data:" << currentDataFrame << "addr:" << s8Data_.dataPos << "l:" << s8Data_.length);
174
175 size_t dataSize = sizeof(Stream8Position);
176 raws8hdFile.writeRecord(currentRoadtime_, 0, reinterpret_cast<const char *>(&s8Data_), dataSize);
177 dataFile_.write(reinterpret_cast<char*>(currentDataFrame), currentDataFrameLength_);
178 }
179
180 // Decoding
181 char * buffer = new char[currentDataFrameLength_ + 1];
182 memcpy(buffer, currentDataFrame, currentDataFrameLength_);
183 buffer[currentDataFrameLength_] = '\0'; // add a \0 to convert for the conversion in QString
184
185 currentDataFrame = buffer;
186 currentRoadtime = currentRoadtime_;
187 currentTimerange = currentRoadtime_;
188
189 analyzeFrame();
190
191 if (newFrameToDecode) {
192 LOG_DEBUG("got new frame to decode");
193
194 type = frameType(frameToDecode);
195 LOG_DEBUG("frame type=" << type);
196 if (-1 != type) {
197 if (-1 == decodeFrame(type)) {
198 LOG_WARN("cannot decode the dataframe");
199 } else {
200 switch(type) {
201 case TRAME_GGA_DBL:
202 if (mVerbose) {
203 cout << "[SPAN-GGA] Lat: " <<
204 ggaFrame_.lat<<
205 " Lon: " << ggaFrame_.lon<<
206 " Hgt: " << ggaFrame_.alt_msl <<
207 " Quality: " << ggaFrame_.ind_qualite << "\n";}
208 break;
209
210 case TRAME_BESTGPSPOSA:
211 //ShMem buffer data copy
212 memcpy(&a.frame, &bestgpsposaFrame_, sizeof(trame_bestgpsposa));
213 a.time=timeOfFrameToDecode;
214 a.timerange=timerangeOfFrameToDecode;
215 memcpy(allFramesBuffer, &a, sizeof(TimestampedBestgpsposaFrame));
216
217 if (recording) {
218 size_t dataSize = sizeof(bestgpsposaFrame_);
219 bestgpsposaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
220 reinterpret_cast<const char *>(&bestgpsposaFrame_), dataSize);
221 }
222 LOG_DEBUG("[SPAN-BESTGPSPOSA]" << "\t"
223 << "Lat=" << bestgpsposaFrame_.Lat << "\t"
224 << "Lon=" << bestgpsposaFrame_.Lon << "\t"
225 << "Hgt=" << bestgpsposaFrame_.Hgt << "\t"
226 << "E=" << bestgpsposaFrame_.e << "\t"
227 << "N=" << bestgpsposaFrame_.n << "\t"
228 << "U=" << bestgpsposaFrame_.u) ;
229
230 mShMem->write(allFramesBuffer, AllFramesSize);
231
232 GET_OUTPUT("bestposegpsposa",CPTComponent,TimestampedBestgpsposaFrame)->send(a);
233
234 break;
235
236 case TRAME_RAWIMUSA:
237
238 //ShMem buffer data copy
239 memcpy(&b.frame, &rawimuFrame_, sizeof(trame_rawimusa));
240 b.time=timeOfFrameToDecode;
241 b.timerange=timerangeOfFrameToDecode;
242 memcpy(allFramesBuffer+
243 sizeof(TimestampedBestgpsposaFrame),&b,sizeof(TimestampedRawimusaFrame));
244
245 if (recording) {
246 size_t dataSize = sizeof(rawimuFrame_);
247 rawimuFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
248 reinterpret_cast<const char *>(&rawimuFrame_), dataSize);
249 }
250 LOG_DEBUG("[SPAN-RAWIMUSA]");
251
252
253 //ShMem refresh
254 mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
255 +sizeof(TimestampedRawimusaFrame)
256 +sizeof(TimestampedInspvaaFrame)
257 +sizeof(TimestampedInscovFrame));
258 GET_OUTPUT("rawimusa",CPTComponent,TimestampedRawimusaFrame)->send(b);
259 break;
260 case TRAME_INSPVAA:
261
262 //ShMem buffer data copy
263 memcpy(&c.frame, &inspvaFrame_, sizeof(trame_inspvaa));
264 c.time=timeOfFrameToDecode;
265 c.timerange=timerangeOfFrameToDecode;
266 memcpy(allFramesBuffer+
267 sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame),
268 &c,sizeof(TimestampedInspvaaFrame));
269
270 if (recording) {
271 size_t dataSize = sizeof(inspvaFrame_);
272 inspvaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
273 reinterpret_cast<const char *>(&inspvaFrame_), dataSize);
274 }
275 LOG_DEBUG("[SPAN-INSPVAA]:" << "\t"
276 << "Lat: " << inspvaFrame_.Lat << "\t"
277 << "Lon: " << inspvaFrame_.Lon << "\t"
278 << "Roll: "<< inspvaFrame_.Roll << "\t"
279 << "Pitch: " << inspvaFrame_.Azimuth << "\t"
280 << "E: " << inspvaFrame_.e << "\t"
281 << "N: " << inspvaFrame_.n << "\t"
282 << "U: " << inspvaFrame_.u);
283
284 //ShMem refresh
285 mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
286 +sizeof(TimestampedRawimusaFrame)
287 +sizeof(TimestampedInspvaaFrame)
288 +sizeof(TimestampedInscovFrame));
289
290 GET_OUTPUT("inspvaaa",CPTComponent,TimestampedInspvaaFrame)->send(c);
291
292 genericEnuPose_.time = c.time;
293 genericEnuPose_.timerange = c.timerange;
294 genericEnuPose_.position = QVector3D(inspvaFrame_.e, inspvaFrame_.n, inspvaFrame_.u);
295 genericEnuPose_.angle = QVector3D((inspvaFrame_.Azimuth+90.0)*(M_PI/180.0),inspvaFrame_.Pitch*(M_PI/180.0),inspvaFrame_.Roll*(M_PI/180.0));
296
297 // for (int i=0;i<3;i++) {
298 // for(int j=0;j<3;j++) {
299 // pose.posconv[i][j] = d.frame.PosCov[i][j];
300 // pose.angcov[i][j] = d.frame.AttCov[i][j];
301 // }
302 // }
303 GET_OUTPUT("genericpose",CPTComponent,Pose3D)->send(genericEnuPose_);
304
305 break;
306 case TRAME_INSCOV:
307
308 //ShMem data copy
309 memcpy(&d.frame, &inscovFrame_, sizeof(trame_inscov));
310 d.time=timeOfFrameToDecode;
311 d.timerange=timerangeOfFrameToDecode;
312 memcpy(allFramesBuffer+
313 sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame)+sizeof(TimestampedInspvaaFrame),
314 &d,sizeof(TimestampedInscovFrame));
315
316 if (recording) {
317 size_t dataSize = sizeof(inscovFrame_);
318 inscovFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
319 reinterpret_cast<const char *>(&inscovFrame_), dataSize);
320 // FIXED: was inscovFile_ instead of inscovFrame_ given as buffer
321 }
322 LOG_DEBUG("[SPAN-INSCOV] CovXX: " << inscovFrame_.PosCov[0][0] <<
323 " CovYY: " << inscovFrame_.PosCov[1][1] <<
324 " CovZZ: "<< inscovFrame_.PosCov[2][2]);
325
326 //ShMem refresh
327 mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
328 +sizeof(TimestampedRawimusaFrame)
329 +sizeof(TimestampedInspvaaFrame)
330 +sizeof(TimestampedInscovFrame));
331
332 GET_OUTPUT("inscov",CPTComponent,TimestampedInscovFrame)->send(d);
333 break;
334 default:
335 break;
336 }
337 }
338 }
339
340 type = -1;
341 newFrameToDecode = false;
342 frameToDecode = "";
343 }
344 delete[] buffer;
345 delete[] currentDataFrame;
346
347 memcpy(mAllFramesBuffer, allFramesBuffer, AllFramesSize);
348
349 delete[] allFramesBuffer;
350
351}
352
353/************************************************************************/
354/* Method to stop the thread */
355/************************************************************************/
356void CPTComponent::stopActivity()
357{
358 serialPort->THREAD_ALIVE = false;
359 if (!serialPort->wait(2000)) {
360 serialPort->terminate();
361 LOG_WARN("The SerialPort thread blocks anormally, it has been killed !!");
362 }
363 if ( !serialPort->closePort() ) {
364 LOG_ERROR("Failed to close the port");
365 } else
366 LOG_INFO("The port is closed");
367 delete serialPort;
368 serialPort = NULL;
369
370 if (recording) {
371 bestgpsposaFile_.close();
372 rawimuFile_.close();
373 inspvaFile_.close();
374 inscovFile_.close();
375 raws8hdFile.close();
376 dataFile_.close();
377 }
378}
379
380/************************************************************************/
381/* Method to start the thread */
382/************************************************************************/
383void CPTComponent::startActivity()
384{
385 type = -1;
386
387#if WIN32
388 serialPort = new Win32SerialPort(portName.toLatin1());
389 // Asynchrone
390 serialPort->setMode(FILE_FLAG_OVERLAPPED);
391 // Synchrone
392 //serialPort->setMode(0);
393#else
394 serialPort = new PosixSerialPort(portName.toLatin1());
395#endif
396
397 if (!serialPort->openPort(portName.toLatin1())) {
398 LOG_ERROR("cannot open the port " << portName);
399 LOG_INFO("cannot start SPAN CPT Component '" << componentName << "'");
400 return;
401 }
402
403 serialPort->configurePort(param.getProperty("baudrate").toLong(),
404 param.getProperty("bytesize").toUInt(),
405 param.getProperty("parity").toShort(),
406 param.getProperty("stopbits").toUInt() - 1);
407
408 if (recording) {
409 bestgpsposaFile_.open("SPAN_bestgpsposa.dbt", WriteMode, TRAME_BESTGPSPOSA, sizeof(trame_bestgpsposa));
410 rawimuFile_.open("SPAN_rawimusa.dbt", WriteMode, TRAME_RAWIMUSA, sizeof(trame_rawimusa));
411 inspvaFile_.open("SPAN_inspvaa.dbt", WriteMode, TRAME_INSPVAA, sizeof(trame_inspvaa));
412 inscovFile_.open("SPAN_inscov.dbt", WriteMode, TRAME_INSCOV, sizeof(trame_inscov));
413
414 // raw data file: Dbt header + binary data stream
415 raws8hdFile.open((ComponentBase::componentName + ".dbt").toStdString(), WriteMode, STREAM8POSITION, sizeof(Stream8Position));
416 // FIXME: use ofstream
417 // open the file with C function to be sure that it will exist
418 FILE * stream = NULL;
419 if ( ( stream = fopen((ComponentBase::componentName + ".utc").toLatin1().data(),"a+") ) == NULL ) {
420 LOG_FATAL("Error while opening the stream utc file\n press a ENTER to exit");
421 getchar();
422 ::exit(-1);
423 } else {
424 fclose(stream);
425 }
426
427 dataFile_.open((ComponentBase::componentName + ".utc").toLatin1().data(), ios_base::out|ios_base::binary|ios_base::app);
428 if (!dataFile_)
429 {
430 LOG_ERROR("Error while opening the file alasca_data.utc\n press a ENTER to exit\n");
431 getchar();
432 ::exit(0);
433 }
434 }
435
436 if (!connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(processing(int)))) {
437 LOG_WARN("cannot connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)");
438 }
439 LOG_INFO("started component '" << componentName << "'");
440
441 serialPort->THREAD_ALIVE = true;
442 serialPort->start();
443
444}
445
446int CPTComponent::analyzeFrame()
447{
448 // SOF = start of frame
449 // EOF = end of frame
450
451 // PPS case
452 //if (currentDataFrame == "PPS")
453 //{
454 // frameToDecode = currentDataFrame;
455 // newFrameToDecode = true;
456 // timeOfFrameToDecode = currentRoadtime;
457 // timerangeOfFrameToDecode = currentTimerange;
458 // return 1;
459 //}
460
461 currentDataFrame = restOfFrame + currentDataFrame;
462 restOfFrame = "";
463
464 if (currentDataFrame.indexOf(QRegExp("[$#%]"),0)!=-1) {
465 sofIdx_ = currentDataFrame.indexOf(QRegExp("[$#%]"),0);
466 startOfFrame = true;
467 timeOfFrameToDecode = currentRoadtime;
468
469 if (currentDataFrame.indexOf("\n",sofIdx_)!=-1) {
470 eofIdx_ = currentDataFrame.indexOf("\n",sofIdx_);
471 endOfFrame = true;
472 timerangeOfFrameToDecode = currentTimerange;
473 }
474 else
475 restOfFrame = currentDataFrame;
476 }
477 else
478 restOfFrame = currentDataFrame;
479
480 if ( (startOfFrame) && (endOfFrame) )
481 {
482 newFrameToDecode = true;
483 for (int i=sofIdx_;i<eofIdx_;i++)
484 frameToDecode +=currentDataFrame.at(i);
485 for (int j=eofIdx_+1; j<currentDataFrame.size(); j++)
486 restOfFrame +=currentDataFrame.at(j);
487 if (mVerbose) {
488 LOG_DEBUG("[Frame:] " << frameToDecode);
489 }
490 }
491 startOfFrame = false;
492 endOfFrame = false;
493 sofIdx_ =0;
494 eofIdx_ =0;
495
496 return 1;
497}
498
499int CPTComponent::frameType(const QString frame)
500{
501 if (( frame[0] == 'P' ) && (frame[1] == 'P')&&( frame[2] == 'S' )) return SIGNAL_PPS ;
502 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'G')&&(frame[5] == 'A')) return TRAME_GGA_DBL ;
503 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'A')) return TRAME_GSA ;
504 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'H' ) && (frame[4] == 'D')&&(frame[5] == 'T')) return TRAME_HDT ;
505 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'T')) return TRAME_GST ;
506 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'V')) return TRAME_GSV ;
507 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'M')&&(frame[5] == 'C')) return TRAME_RMC ;
508 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'O')&&(frame[5] == 'T')) return TRAME_ROT ;
509 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'V' ) && (frame[4] == 'T')&&(frame[5] == 'G')) return TRAME_VTG ;
510 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'Z' ) && (frame[4] == 'D')&&(frame[5] == 'A')) return TRAME_ZDA ;
511 if ((frame[0]== '#')||(frame[0]== '%')) { // Span CPT frames
512 if ((frame.contains("BESTGPSPOSA", Qt::CaseSensitive)))
513 return TRAME_BESTGPSPOSA;
514 else if ((frame.contains("RAWIMUSA", Qt::CaseSensitive)))
515 return TRAME_RAWIMUSA;
516 else if ((frame.contains("INSPVAA", Qt::CaseSensitive))||(frame.contains("INSPVASA", Qt::CaseSensitive)))
517 return TRAME_INSPVAA;
518 else if ((frame.contains("INSCOVA", Qt::CaseSensitive))||(frame.contains("INSCOVSA", Qt::CaseSensitive)))
519 return TRAME_INSCOV;
520 else
521 return UNKNOWN_NMEA_FRAME;
522 }
523 return UNKNOWN_NMEA_FRAME;
524}
525
526int CPTComponent::decodeFrame(int type)
527{
528 double lat_rad = 0, lon_rad = 0;
529 int indexGSV = 0;
530 int indexGSA = 0;
531 //int pps = 1; // dummy value for pps recording
532
533 SENTENCE sentence;
534 sentence.Sentence = frameToDecode;
535
536 QStringList stringList;
537 QString header;
538 QString data;
539
540 switch(type) {
541 case UNKNOWN_NMEA_FRAME:
542 LOG_ERROR("received unknown frame");
543 break;
544
545 case SIGNAL_PPS:
546 // TODO
547 LOG_WARN("unimplemented");
548 break;
549
550 case TRAME_GGA_DBL:
551 if (sentence.Sentence.contains("PPS", Qt::CaseSensitive)) {
552 LOG_ERROR("Almanac error on SPAN CPT");
553 return -1;
554 } else {
555 if (!nmea0183_.Gga.Parse(sentence)) {
556 LOG_ERROR("cannot parse the frame " << nmea0183_.Gga.ErrorMessage);
557 }
558 lat_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Latitude.GetDecimalDegrees());
559 lon_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Longitude.GetDecimalDegrees());
560 ggaFrame_.H = nmea0183_.Gga.Time.time().hour();
561 ggaFrame_.Mi = nmea0183_.Gga.Time.time().minute();
562 ggaFrame_.S = nmea0183_.Gga.Time.time().second();
563 ggaFrame_.Ms = nmea0183_.Gga.Time.time().msec();
564 ggaFrame_.lon = lon_rad;
565 ggaFrame_.lat = lat_rad;
566 ggaFrame_.ind_qualite = nmea0183_.Gga.GPSQuality;
567 ggaFrame_.nb_sat = nmea0183_.Gga.NumberOfSatellitesInUse;
568 ggaFrame_.hdop = nmea0183_.Gga.HorizontalDilutionOfPrecision;
569 ggaFrame_.alt_msl = nmea0183_.Gga.AntennaAltitudeMeters;
570 ggaFrame_.d_geoidal = nmea0183_.Gga.GeoidalSeparationMeters;
571 ggaFrame_.age = nmea0183_.Gga.AgeOfDifferentialGPSDataSeconds;
572 ggaFrame_.dir_lat = ( (nmea0183_.Gga.Position.Latitude.Northing == North) ? 'N' : 'S' );
573 ggaFrame_.dir_lon = ( (nmea0183_.Gga.Position.Longitude.Easting == East) ? 'E' : 'W' );
574 ggaFrame_.ref_station_ID = nmea0183_.Gga.DifferentialReferenceStationID;
575 }
576 break;
577
578 case TRAME_GSA:
579 if (!nmea0183_.Gsa.Parse(sentence))
580 LOG_WARN("Failed to parse the frame " << nmea0183_.Gsa.ErrorMessage.toLatin1().data());
581 gsaFrame_.mode_select = ((nmea0183_.Gsa.OperatingMode == GSA::Manual) ? 'M' : 'A');
582 gsaFrame_.mode_result = 0;
583 if (nmea0183_.Gsa.FixMode == GSA::FixUnavailable)
584 gsaFrame_.mode_result = 1;
585 if (nmea0183_.Gsa.FixMode == GSA::TwoDimensional)
586 gsaFrame_.mode_result = 2;
587 if (nmea0183_.Gsa.FixMode == GSA::ThreeDimensional)
588 gsaFrame_.mode_result = 3;
589 for (indexGSA = 0 ; indexGSA<12 ; indexGSA++)
590 gsaFrame_.SV_PRN[indexGSA] = nmea0183_.Gsa.SatelliteNumber[indexGSA];
591 gsaFrame_.pdop = nmea0183_.Gsa.PDOP;
592 gsaFrame_.hdop = nmea0183_.Gsa.HDOP;
593 gsaFrame_.vdop = nmea0183_.Gsa.VDOP;
594
595 break;
596
597 case TRAME_GST:
598 if (!nmea0183_.Gst.Parse( sentence ))
599 LOG_WARN("Failed to parse the frame " << nmea0183_.Gst.ErrorMessage.toLatin1().data());
600 gstFrame_.rms = nmea0183_.Gst.RMSvalue;
601 gstFrame_.a = nmea0183_.Gst.ErrorEllipseMajor;
602 gstFrame_.b = nmea0183_.Gst.ErrorEllipseMinor;
603 gstFrame_.phi = nmea0183_.Gst.ErrorEllipseOrientation;
604 gstFrame_.sigma_lat = nmea0183_.Gst.LatitudeError;
605 gstFrame_.sigma_lon = nmea0183_.Gst.LongitudeError;
606 gstFrame_.sigma_alt = nmea0183_.Gst.HeightError;
607 gstFrame_.H = nmea0183_.Gst.Time.time().hour();
608 gstFrame_.Mi = nmea0183_.Gst.Time.time().minute();
609 gstFrame_.S = nmea0183_.Gst.Time.time().second();
610 gstFrame_.Ms = nmea0183_.Gst.Time.time().msec();
611
612 break;
613
614 case TRAME_GSV:
615 indexGSV = 0;
616 if (!nmea0183_.Gsv.Parse( sentence ))
617 LOG_WARN("Failed to parse the frame " << nmea0183_.Gsv.ErrorMessage.toLatin1().data());
618 // it's a new frame, reset stored value in case of the number of satellites
619 // in view has decreased
620 if (nmea0183_.Gsv.message_number == 1)
621 {
622 while (indexGSV < 36)
623 {
624 gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = 0;
625 gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = 0;
626 gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = 0;
627 gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = 0;
628 indexGSV++;
629 }
630 }
631 gsvFrame_.NumberOfSatellites = nmea0183_.Gsv.NumberOfSatellites;
632 gsvFrame_.Totalmessages = nmea0183_.Gsv.Totalmessages;
633
634 for ( indexGSV=4*(nmea0183_.Gsv.message_number-1); indexGSV<=(4*nmea0183_.Gsv.message_number)-1; indexGSV++ )
635 {
636 gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SatelliteNumber;
637 gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].ElevationDegrees;
638 gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].AzimuthDegreesTrue;
639 gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SignalToNoiseRatio;
640 }
641
642 break;
643
644 case TRAME_HDT:
645 if (!nmea0183_.Hdt.Parse( sentence ))
646 LOG_WARN("Failed to parse the frame " << nmea0183_.Hdt.ErrorMessage.toLatin1().data());
647 hdtFrame_.DegreesTrue = nmea0183_.Hdt.DegreesTrue;
648
649 break;
650
651 case TRAME_RMC:
652 if (!nmea0183_.Rmc.Parse( sentence ))
653 LOG_WARN("Failed to parse the frame " << nmea0183_.Rmc.ErrorMessage.toLatin1().data());
654 rmcFrame_.H = nmea0183_.Rmc.Time.time().hour();
655 rmcFrame_.Mi = nmea0183_.Rmc.Time.time().minute();
656 rmcFrame_.S = nmea0183_.Rmc.Time.time().second();
657 rmcFrame_.Ms = nmea0183_.Rmc.Time.time().msec();
658 rmcFrame_.AA = nmea0183_.Rmc.Time.date().year();
659 rmcFrame_.MM = nmea0183_.Rmc.Time.date().month();
660 rmcFrame_.JJ = nmea0183_.Rmc.Time.date().day();
661 rmcFrame_.lat = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Latitude.GetDecimalDegrees());
662 rmcFrame_.dir_lat = ( (nmea0183_.Rmc.Position.Latitude.Northing == North) ? 'N' : 'S');
663 rmcFrame_.lon = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Longitude.GetDecimalDegrees());
664 rmcFrame_.dir_lon = ( (nmea0183_.Rmc.Position.Longitude.Easting == East) ? 'E' : 'W' );
665 rmcFrame_.magnet_var = nmea0183_.Rmc.MagneticVariation;
666 rmcFrame_.dir_magnet_var = ( (nmea0183_.Rmc.MagneticVariationDirection == East) ? 'E' : 'W');
667 rmcFrame_.mode = -1;
668 if (nmea0183_.Rmc.ModeIndication == "A")
669 rmcFrame_.mode = 1;
670 if (nmea0183_.Rmc.ModeIndication == "D")
671 rmcFrame_.mode = 2;
672 if (nmea0183_.Rmc.ModeIndication == "N")
673 rmcFrame_.mode = 0;
674 rmcFrame_.track_true_north = nmea0183_.Rmc.TrackMadeGoodDegreesTrue;
675 rmcFrame_.valid_data = ( (nmea0183_.Rmc.IsDataValid == True) ? 1 : 0 );
676 rmcFrame_.vitesse = nmea0183_.Rmc.SpeedOverGroundKnots * 1852.0 / 3600.0; // 1 knot = 1852 m/h
677
678 break;
679
680 case TRAME_ROT:
681 if (!nmea0183_.Rot.Parse( sentence ))
682 LOG_WARN("Failed to parse the frame " << nmea0183_.Rot.ErrorMessage.toLatin1().data());
683 rotFrame_.RateOfTurn = nmea0183_.Rot.RateOfTurn;
684 rotFrame_.valid_data = ( (nmea0183_.Rot.IsDataValid == True) ? 1 : 0 );
685
686 break;
687
688 case TRAME_VTG:
689 if (!nmea0183_.Vtg.Parse( sentence ))
690 LOG_WARN("Failed to parse the frame " << nmea0183_.Vtg.ErrorMessage.toLatin1().data());
691 vtgFrame_.v = nmea0183_.Vtg.SpeedKilometersPerHour;
692 vtgFrame_.track_true_north = nmea0183_.Vtg.TrackDegreesTrue;
693 vtgFrame_.track_magnet_north = nmea0183_.Vtg.TrackDegreesMagnetic;
694
695 break;
696
697 case TRAME_ZDA:
698 if (!nmea0183_.Zda.Parse( sentence ))
699 LOG_WARN("Failed to parse the frame " << nmea0183_.Zda.ErrorMessage.toLatin1().data());
700 zdaFrame_.H = nmea0183_.Zda.Time.time().hour();
701 zdaFrame_.Mi = nmea0183_.Zda.Time.time().minute();
702 zdaFrame_.S = nmea0183_.Zda.Time.time().second();
703 zdaFrame_.Ms = nmea0183_.Zda.Time.time().msec();
704 zdaFrame_.AA = nmea0183_.Zda.Time.date().year();
705 zdaFrame_.MM = nmea0183_.Zda.Time.date().month();
706 zdaFrame_.JJ = nmea0183_.Zda.Time.date().day();
707 zdaFrame_.H_offset = nmea0183_.Zda.LocalHourDeviation;
708 zdaFrame_.Mi_offset = nmea0183_.Zda.LocalMinutesDeviation;
709
710 break;
711
712 case TRAME_BESTGPSPOSA:
713 stringList = sentence.Sentence.split(";");
714 if (stringList.size()==2) {
715 header = stringList.at(0);
716 data = stringList.at(1);
717
718 if (mVerbose) {
719 LOG_DEBUG("Header " << stringList.at(0));
720 LOG_DEBUG("Data " << stringList.at(1));
721 }
722
723 if (parseCPTbestgpsposa(data)) {
724 if (mVerbose) {
725 LOG_DEBUG("parsed TRAME_BESTGPSPOSA");
726 }
727 }
728 }
729 break;
730
731 case TRAME_RAWIMUSA:
732 stringList = sentence.Sentence.split(";");
733 LOG_DEBUG(sentence.Sentence);
734 if (stringList.size()==2) {
735 header = stringList.at(0);
736 data = stringList.at(1);
737
738 LOG_DEBUG("Header " << stringList.at(0));
739 LOG_DEBUG("Data " << stringList.at(1));
740
741 if (parseCPTrawimusa(data))
742 LOG_DEBUG("parsed TRAME_RAWIMUSA");
743 }
744 break;
745 case TRAME_INSPVAA:
746 stringList = sentence.Sentence.split(";");
747 LOG_DEBUG(sentence.Sentence);
748 if (stringList.size()==2) {
749 header = stringList.at(0);
750 data = stringList.at(1);
751
752 LOG_DEBUG("Header " << stringList.at(0));
753 LOG_DEBUG("Data " << stringList.at(1));
754
755 if (parseCPTinspvaa(data))
756 LOG_DEBUG("parsed TRAME_INSPVAA");
757
758
759 }
760 break;
761
762 case TRAME_INSCOV:
763 stringList = sentence.Sentence.split(";");
764 LOG_DEBUG(sentence.Sentence);
765 if (stringList.size()==2) {
766 header = stringList.at(0);
767 data = stringList.at(1);
768
769 LOG_DEBUG("Header " << stringList.at(0));
770 LOG_DEBUG("Data " << stringList.at(1));
771
772 if (parseCPTinscov(data))
773 LOG_DEBUG("parsed TRAME_INSCOV");
774
775 }
776 break;
777
778 default:
779 LOG_ERROR("unknown frame type=" << type);
780 return 0;
781 }
782
783 return 1;
784}
785
786int CPTComponent::parseCPTbestgpsposa(QString data)
787{
788 double x=0.0;
789 double y=0.0;
790 double z=0.0;
791
792 QStringList stringList = data.split(",");
793 if (21 == stringList.size()) {
794 if (stringList.at(0).compare("SOL_COMPUTED")==0)
795 bestgpsposaFrame_.Status = SOL_COMPUTED;
796 else if (stringList.at(0).compare("INSUFFICIENT_OBS")==0)
797 bestgpsposaFrame_.Status = INSUFFICIENT_OBS;
798 else if (stringList.at(0).compare("NO_CONVERGENCE")==0)
799 bestgpsposaFrame_.Status = INSUFFICIENT_OBS;
800 else if (stringList.at(0).compare("SINGULARITY")==0)
801 bestgpsposaFrame_.Status = SINGULARITY;
802 else if (stringList.at(0).compare("COV_TRACE ")==0)
803 bestgpsposaFrame_.Status = COV_TRACE;
804 else if (stringList.at(0).compare("TEST_DIST")==0)
805 bestgpsposaFrame_.Status = TEST_DIST;
806 else if (stringList.at(0).compare("COLD_START")==0)
807 bestgpsposaFrame_.Status = COLD_START;
808 else if (stringList.at(0).compare("V_H_LIMIT")==0)
809 bestgpsposaFrame_.Status = V_H_LIMIT;
810 else if (stringList.at(0).compare("VARIANCE")==0)
811 bestgpsposaFrame_.Status = VARIANCE;
812 else if (stringList.at(0).compare("RESIDUALS")==0)
813 bestgpsposaFrame_.Status = RESIDUALS;
814 else if (stringList.at(0).compare("DELTA_POS")==0)
815 bestgpsposaFrame_.Status = DELTA_POS;
816 else if (stringList.at(0).compare("NEGATIVE_VAR")==0)
817 bestgpsposaFrame_.Status = NEGATIVE_VAR;
818 else if (stringList.at(0).compare("INTEGRITY_WARNING")==0)
819 bestgpsposaFrame_.Status = INTEGRITY_WARNING;
820 else if (stringList.at(0).compare("IMU_UNPLUGGED")==0)
821 bestgpsposaFrame_.Status = IMU_UNPLUGGED;
822 else if (stringList.at(0).compare("PENDING")==0)
823 bestgpsposaFrame_.Status = PENDING;
824 else {
825 LOG_ERROR("cannot parse BESTGPSPOSA");
826 return 0;
827 }
828
829 if (stringList.at(1).compare("NONE")==0) {
830 bestgpsposaFrame_.posType = NONE;
831 } else if (stringList.at(1).compare("FIXEDPOS")==0) {
832 bestgpsposaFrame_.posType = FIXEDPOS;
833 } else if (stringList.at(1).compare("FIXEDHEIGHT")==0) {
834 bestgpsposaFrame_.posType = FIXEDHEIGHT;
835 } else if (stringList.at(1).compare("FLOATCONV")==0) {
836 bestgpsposaFrame_.posType = FLOATCONV;
837 } else if (stringList.at(1).compare("WIDELANE")==0) {
838 bestgpsposaFrame_.posType = WIDELANE;
839 } else if (stringList.at(1).compare("NARROWLANE")==0) {
840 bestgpsposaFrame_.posType = NARROWLANE;
841 } else if (stringList.at(1).compare("DOPPLER_VELOCITY")==0) {
842 bestgpsposaFrame_.posType = DOPPLER_VELOCITY;
843 } else if (stringList.at(1).compare("SINGLE")==0) {
844 bestgpsposaFrame_.posType = SINGLE;
845 } else if (stringList.at(1).compare("PSRDIFF")==0) {
846 bestgpsposaFrame_.posType = PSRDIFF;
847 } else if (stringList.at(1).compare("WAAS")==0) {
848 bestgpsposaFrame_.posType = WAAS;
849 } else if (stringList.at(1).compare("PROPAGATED")==0) {
850 bestgpsposaFrame_.posType = PROPAGATED;
851 } else if (stringList.at(1).compare("OMNISTAR")==0) {
852 bestgpsposaFrame_.posType = OMNISTAR;
853 } else if (stringList.at(1).compare("L1_FLOAT")==0) {
854 bestgpsposaFrame_.posType = L1_FLOAT;
855 } else if (stringList.at(1).compare("IONOFREE_FLOAT")==0) {
856 bestgpsposaFrame_.posType = IONOFREE_FLOAT;
857 } else if (stringList.at(1).compare("NARROW_FLOAT")==0) {
858 bestgpsposaFrame_.posType = NARROW_FLOAT;
859 } else if (stringList.at(1).compare("L1_INT")==0) {
860 bestgpsposaFrame_.posType = L1_INT;
861 } else if (stringList.at(1).compare("WIDE_INT")==0) {
862 bestgpsposaFrame_.posType = WIDE_INT;
863 } else if (stringList.at(1).compare("NARROW_INT")==0) {
864 bestgpsposaFrame_.posType = NARROW_INT;
865 } else if (stringList.at(1).compare("RTK_DIRECT_INS")==0) {
866 bestgpsposaFrame_.posType = RTK_DIRECT_INS;
867 } else if (stringList.at(1).compare("INS")==0) {
868 bestgpsposaFrame_.posType = INS;
869 } else if (stringList.at(1).compare("INS_PSRSP")==0) {
870 bestgpsposaFrame_.posType = INS_PSRSP;
871 } else if (stringList.at(1).compare("INS_PSRDIFF")==0) {
872 bestgpsposaFrame_.posType = INS_PSRDIFF;
873 } else if (stringList.at(1).compare("INS_RTKFLOAT")==0) {
874 bestgpsposaFrame_.posType = INS_RTKFLOAT;
875 } else if (stringList.at(1).compare("INS_RTKFIXED")==0) {
876 bestgpsposaFrame_.posType = INS_RTKFIXED;
877 } else if (stringList.at(1).compare("INS_OMNISTAR")==0) {
878 bestgpsposaFrame_.posType = INS_OMNISTAR;
879 } else if (stringList.at(1).compare("INS_OMNISTAR_HP")==0) {
880 bestgpsposaFrame_.posType = INS_OMNISTAR_HP;
881 } else if (stringList.at(1).compare("INS_OMNISTAR_XP")==0) {
882 bestgpsposaFrame_.posType = INS_OMNISTAR_XP;
883 } else if (stringList.at(1).compare("OMNISTAR_HP")==0) {
884 bestgpsposaFrame_.posType = OMNISTAR_HP;
885 } else if (stringList.at(1).compare("OMNISTAR_XP")==0) {
886 bestgpsposaFrame_.posType = OMNISTAR_XP;
887 } else if (stringList.at(1).compare("CDGPS")==0) {
888 bestgpsposaFrame_.posType = CDGPS;
889 } else {
890 LOG_ERROR("cannot parse BESTGPSPOSA");
891 return 0;
892 }
893
894 bestgpsposaFrame_.Lat = stringList.at(2).toDouble();
895 bestgpsposaFrame_.Lon = stringList.at(3).toDouble();
896 bestgpsposaFrame_.Hgt = stringList.at(4).toDouble();
897 bestgpsposaFrame_.Undulation = stringList.at(5).toFloat();
898 bestgpsposaFrame_.LatStd = stringList.at(7).toFloat();
899 bestgpsposaFrame_.LonStd = stringList.at(8).toFloat();
900 bestgpsposaFrame_.HgtStd = stringList.at(9).toFloat();
901
902 // Geo to ECEF
903 Geodesie::Geographique_2_ECEF(
904 Geodesie::Deg2Rad(bestgpsposaFrame_.Lon), Geodesie::Deg2Rad(bestgpsposaFrame_.Lat), bestgpsposaFrame_.Hgt,
905 x, y, z);
906 // ECEF to ENU
907 Geodesie::ECEF_2_ENU(x, y, z,
908 bestgpsposaFrame_.e, bestgpsposaFrame_.n, bestgpsposaFrame_.u,
909 Geodesie::Deg2Rad(LON_REF), Geodesie::Deg2Rad(LAT_REF), HE_REF);
910 return 1;
911 }
912 return 0;
913}
914
915int CPTComponent::parseCPTrawimusa(QString data)
916{
917 QStringList stringList = data.split(",");
918 QStringList stringList2;
919 if (9 == stringList.size()) {
920 rawimuFrame_.Week = stringList.at(0).toULong();
921 rawimuFrame_.Seconds = stringList.at(1).toDouble();
922
923 rawimuFrame_.ZAccel = stringList.at(3).toLong();
924 rawimuFrame_.YAccel = stringList.at(4).toLong();
925 rawimuFrame_.XAccel = stringList.at(5).toLong();
926
927 rawimuFrame_.ZGyro = stringList.at(6).toLong();
928 rawimuFrame_.YGyro = stringList.at(7).toLong();
929
930 stringList2 = stringList.at(8).split("*");
931 if (2 == stringList2.size()) {
932 rawimuFrame_.XGyro = stringList2.at(0).toLong();
933 }
934 return 1;
935 }
936 return 0;
937}
938
939int CPTComponent::parseCPTinspvaa(QString data)
940{
941 QStringList stringList = data.split(",");
942 QStringList stringList2;
943
944 double x=0.0;
945 double y=0.0;
946 double z=0.0;
947
948 if (12 == stringList.size()) {
949 inspvaFrame_.Week = stringList.at(0).toULong();
950 inspvaFrame_.Seconds = stringList.at(1).toDouble();
951 inspvaFrame_.Lat = stringList.at(2).toDouble();
952 inspvaFrame_.Lon = stringList.at(3).toDouble();
953 inspvaFrame_.Hgt = stringList.at(4).toDouble();
954 inspvaFrame_.NorthVel = stringList.at(5).toFloat();
955 inspvaFrame_.EastVel = stringList.at(6).toFloat();
956 inspvaFrame_.UpVel = stringList.at(7).toFloat();
957 inspvaFrame_.Roll = stringList.at(8).toFloat();
958 inspvaFrame_.Pitch = stringList.at(9).toFloat();
959 inspvaFrame_.Azimuth = stringList.at(10).toFloat();
960
961 // Geo to ECEF
962 Geodesie::Geographique_2_ECEF(
963 Geodesie::Deg2Rad(inspvaFrame_.Lon), Geodesie::Deg2Rad(inspvaFrame_.Lat), inspvaFrame_.Hgt,
964 x, y, z);
965 // ECEF to ENU
966 Geodesie::ECEF_2_ENU(x, y, z,
967 inspvaFrame_.e, inspvaFrame_.n, inspvaFrame_.u,
968 Geodesie::Deg2Rad(LON_REF), Geodesie::Deg2Rad(LAT_REF), HE_REF);
969
970 stringList2 = stringList.at(11).split("*");
971 if (2 == stringList2.size()) {
972 if (stringList2.at(0).compare("INS_INACTIVE")==0)
973 inspvaFrame_.Status = INS_INACTIVE;
974 else if (stringList2.at(0).compare("INS_ALIGNING")==0)
975 inspvaFrame_.Status = INS_ALIGNING;
976 else if (stringList2.at(0).compare("INS_SOLUTION_NOT_GOOD")==0)
977 inspvaFrame_.Status = INS_SOLUTION_NOT_GOOD;
978 else if (stringList2.at(0).compare("INS_SOLUTION_GOOD")==0)
979 inspvaFrame_.Status = INS_SOLUTION_GOOD;
980 else if (stringList2.at(0).compare("INS_BAD_GPS_AGREEMENT")==0)
981 inspvaFrame_.Status = INS_BAD_GPS_AGREEMENT;
982 else if (stringList2.at(0).compare("INS_ALIGNMENT_COMPLETE")==0)
983 inspvaFrame_.Status = INS_ALIGNMENT_COMPLETE;
984
985 return 1;
986 }
987 }
988 return 0;
989}
990
991int CPTComponent::parseCPTinscov(QString data)
992{
993 QStringList stringList = data.split(",");
994 QStringList stringList2;
995 if (29 == stringList.size()) {
996 inscovFrame_.Week = stringList.at(0).toULong();
997 inscovFrame_.Seconds = stringList.at(1).toDouble();
998 //PosCov
999 inscovFrame_.PosCov[0][0]=stringList.at(2).toDouble();
1000 inscovFrame_.PosCov[0][1]=stringList.at(3).toDouble();
1001 inscovFrame_.PosCov[0][2]=stringList.at(4).toDouble();
1002
1003 inscovFrame_.PosCov[1][0]=stringList.at(5).toDouble();
1004 inscovFrame_.PosCov[1][1]=stringList.at(6).toDouble();
1005 inscovFrame_.PosCov[1][2]=stringList.at(7).toDouble();
1006
1007 inscovFrame_.PosCov[2][0]=stringList.at(8).toDouble();
1008 inscovFrame_.PosCov[2][1]=stringList.at(9).toDouble();
1009 inscovFrame_.PosCov[2][2]=stringList.at(10).toDouble();
1010 //AttCov
1011 inscovFrame_.AttCov[0][0]=stringList.at(11).toDouble();
1012 inscovFrame_.AttCov[0][1]=stringList.at(12).toDouble();
1013 inscovFrame_.AttCov[0][2]=stringList.at(13).toDouble();
1014
1015 inscovFrame_.AttCov[1][0]=stringList.at(14).toDouble();
1016 inscovFrame_.AttCov[1][1]=stringList.at(15).toDouble();
1017 inscovFrame_.AttCov[1][2]=stringList.at(16).toDouble();
1018
1019 inscovFrame_.AttCov[2][0]=stringList.at(17).toDouble();
1020 inscovFrame_.AttCov[2][1]=stringList.at(18).toDouble();
1021 inscovFrame_.AttCov[2][2]=stringList.at(19).toDouble();
1022 //VelCov
1023 inscovFrame_.VelCov[0][0]=stringList.at(20).toDouble();
1024 inscovFrame_.VelCov[0][1]=stringList.at(21).toDouble();
1025 inscovFrame_.VelCov[0][2]=stringList.at(22).toDouble();
1026
1027 inscovFrame_.VelCov[1][0]=stringList.at(23).toDouble();
1028 inscovFrame_.VelCov[1][1]=stringList.at(24).toDouble();
1029 inscovFrame_.VelCov[1][2]=stringList.at(25).toDouble();
1030
1031 inscovFrame_.VelCov[2][0]=stringList.at(26).toDouble();
1032 inscovFrame_.VelCov[2][1]=stringList.at(27).toDouble();
1033
1034 stringList2 = stringList.at(11).split("*");
1035 if (2 == stringList2.size()) {
1036 inscovFrame_.VelCov[2][2]=stringList2.at(0).toDouble();
1037 return 1;
1038 }
1039 }
1040 return 0;
1041}
1042
1043void CPTComponent::initBuffer(TimestampedBestgpsposaFrame* a,
1044 TimestampedRawimusaFrame* b,
1045 TimestampedInspvaaFrame* c,
1046 TimestampedInscovFrame* d)
1047{
1048 a->time=0;
1049 a->timerange=0;
1050 a->frame.Status=INSUFFICIENT_OBS;
1051 a->frame.posType=NONE;
1052 a->frame.Lat=0.0;
1053 a->frame.Lon=0.0;
1054 a->frame.Hgt=0.0;
1055 a->frame.Undulation=0.0;
1056 a->frame.LatStd=0.0;
1057 a->frame.LonStd=0.0;
1058 a->frame.HgtStd=0.0;
1059 a->frame.e=0.0;
1060 a->frame.n=0.0;
1061 a->frame.u=0.0;
1062
1063 b->time=0;
1064 b->timerange=0;
1065 b->frame.Week=0;
1066 b->frame.Seconds=0.0;
1067 b->frame.ZAccel=0.0;
1068 b->frame.YAccel=0.0;
1069 b->frame.XAccel=0.0;
1070 b->frame.ZGyro=0.0;
1071 b->frame.YGyro=0.0;
1072 b->frame.XGyro=0.0;
1073
1074 c->time=0;
1075 c->timerange=0;
1076 c->frame.Week =0;
1077 c->frame.Seconds=0.0;
1078 c->frame.Lat=0.0;
1079 c->frame.Lon=0.0;
1080 c->frame.Hgt=0.0;
1081 c->frame.NorthVel=0.0;
1082 c->frame.EastVel=0.0;
1083 c->frame.UpVel=0.0;
1084 c->frame.Roll=0.0;
1085 c->frame.Pitch=0.0;
1086 c->frame.Azimuth=0.0;
1087 c->frame.Status=INS_INACTIVE;
1088 c->frame.e=0.0;
1089 c->frame.n=0.0;
1090 c->frame.u=0.0;
1091
1092 d->time=0;
1093 d->timerange=0;
1094 d->frame.Week=0;
1095 d->frame.Seconds=0.0;
1096 for (register int i=0;i<3;i++) {
1097 for (register int j=0;j<3;j++) {
1098 d->frame.PosCov[i][j]=0.0;
1099 d->frame.AttCov[i][j]=0.0;
1100 d->frame.VelCov[i][j]=0.0;
1101 }
1102 }
1103}
Note: See TracBrowser for help on using the repository browser.