source: pacpussensors/trunk/SpanCPTComponent/CPTComponent.cpp@ 110

Last change on this file since 110 was 105, checked in by cerichar, 9 years ago

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