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

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

Integration of new modules:

  • GPS NMEA0183 decoder
  • Span CPT Decoder

Update of:

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