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

Last change on this file since 90 was 88, checked in by DHERBOMEZ Gérald, 9 years ago

add output of 2D ENU position
File size: 43.1 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
23
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
60 currentDataFrame_ ="";
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
75 mShMem = new ShMem("SPAN_FRAMES", AllFramesSize);
76 mAllFramesBuffer = new char[AllFramesSize];
77}
78
79/// Destructor of the CPTComponent class
80CPTComponent::~CPTComponent()
81{
82 delete mShMem;
83 delete[] mAllFramesBuffer;
84}
85
86ComponentBase::COMPONENT_CONFIGURATION CPTComponent::configureComponent(XmlComponentConfig /*config*/)
87{
88 setPortCOM( xmlParameters().getProperty("port").toLatin1() );
89
90 if (!xmlParameters().getProperty("recording").isNull()) {
91 setRecording ( (xmlParameters().getProperty("recording") == "true" ? true : false) );
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
110
111
112void CPTComponent::addOutputs()
113{
114 // must inherit from QObject to use addOutput
115 addOutput<QPointF, CPTComponent>("position2DENU");
116}
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->getNextFrame();
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 // free memory
166 delete currentFrame;
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
185 currentDataFrame_ = QString::fromAscii(buffer);
186
187 delete[] buffer;
188 delete[] currentDataFrame;
189
190 analyzeFrame();
191
192 if (newFrameToDecode) {
193 LOG_DEBUG("got new frame to decode");
194
195 type = frameType(frameToDecode);
196 LOG_DEBUG("frame type=" << type);
197 if (-1 != type) {
198 if (-1 == decodeFrame(type)) {
199 LOG_WARN("cannot decode the dataframe");
200 } else {
201 switch(type) {
202 case TRAME_GGA_DBL:
203 if (mVerbose) {
204 cout << "[SPAN-GGA] Lat: " <<
205 ggaFrame_.lat<<
206 " Lon: " << ggaFrame_.lon<<
207 " Hgt: " << ggaFrame_.alt_msl <<
208 " Quality: " << ggaFrame_.ind_qualite << "\n";}
209 break;
210
211 case TRAME_BESTGPSPOSA:
212 //ShMem buffer data copy
213 memcpy(&a.frame, &bestgpsposaFrame_, sizeof(trame_bestgpsposa));
214 a.time=timeOfFrameToDecode;
215 a.timerange=timerangeOfFrameToDecode;
216 memcpy(allFramesBuffer, &a, sizeof(TimestampedBestgpsposaFrame));
217
218 if (isRecording()) {
219 size_t dataSize = sizeof(bestgpsposaFrame_);
220 bestgpsposaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
221 reinterpret_cast<const char *>(&bestgpsposaFrame_), dataSize);
222 }
223 if (mVerbose) {
224 cout << "[SPAN-BESTGPSPOSA]" << "\t"
225 << "Lat=" << bestgpsposaFrame_.Lat << "\t"
226 << "Lon=" << bestgpsposaFrame_.Lon << "\t"
227 << "Hgt=" << bestgpsposaFrame_.Hgt << "\t"
228 << "E=" << bestgpsposaFrame_.e << "\t"
229 << "N=" << bestgpsposaFrame_.n << "\t"
230 << "U=" << bestgpsposaFrame_.u
231 ;
232 }
233 mShMem->write(allFramesBuffer, AllFramesSize);
234
235 if (position2DENUOutput_ && position2DENUOutput_->hasConnection())
236 {
237 // send ENU 2D position
238 QPointF pt( bestgpsposaFrame_.e, bestgpsposaFrame_.n);
239 position2DENUOutput_->send(pt);
240 }
241 break;
242
243 case TRAME_RAWIMUSA:
244
245 //ShMem buffer data copy
246 memcpy(&b.frame, &rawimuFrame_, sizeof(trame_rawimusa));
247 b.time=timeOfFrameToDecode;
248 b.timerange=timerangeOfFrameToDecode;
249 memcpy(allFramesBuffer+
250 sizeof(TimestampedBestgpsposaFrame),&b,sizeof(TimestampedRawimusaFrame));
251
252 if (isRecording()) {
253 size_t dataSize = sizeof(rawimuFrame_);
254 rawimuFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
255 reinterpret_cast<const char *>(&rawimuFrame_), dataSize);
256 }
257 if (mVerbose) {
258 cout << "[SPAN-RAWIMUSA]"<< endl;
259 }
260
261 //ShMem refresh
262 mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
263 +sizeof(TimestampedRawimusaFrame)
264 +sizeof(TimestampedInspvaaFrame)
265 +sizeof(TimestampedInscovFrame));
266 break;
267 case TRAME_INSPVAA:
268
269 //ShMem buffer data copy
270 memcpy(&c.frame, &inspvaFrame_, sizeof(trame_inspvaa));
271 c.time=timeOfFrameToDecode;
272 c.timerange=timerangeOfFrameToDecode;
273 memcpy(allFramesBuffer+
274 sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame),
275 &c,sizeof(TimestampedInspvaaFrame));
276
277 if (isRecording()) {
278 size_t dataSize = sizeof(inspvaFrame_);
279 inspvaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
280 reinterpret_cast<const char *>(&inspvaFrame_), dataSize);
281 }
282 if (mVerbose) {
283 cout << "[SPAN-INSPVAA]:" << "\t"
284 << "Lat: " << inspvaFrame_.Lat << "\t"
285 << "Lon: " << inspvaFrame_.Lon << "\t"
286 << "Roll: "<< inspvaFrame_.Roll << "\t"
287 << "Pitch: " << inspvaFrame_.Azimuth << "\t"
288 << "E: " << inspvaFrame_.e << "\t"
289 << "N: " << inspvaFrame_.n << "\t"
290 << "U: " << inspvaFrame_.u << endl;
291 }
292
293 //ShMem refresh
294 mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
295 +sizeof(TimestampedRawimusaFrame)
296 +sizeof(TimestampedInspvaaFrame)
297 +sizeof(TimestampedInscovFrame));
298 break;
299 case TRAME_INSCOV:
300
301 //ShMem data copy
302 memcpy(&d.frame, &inscovFrame_, sizeof(trame_inscov));
303 d.time=timeOfFrameToDecode;
304 d.timerange=timerangeOfFrameToDecode;
305 memcpy(allFramesBuffer+
306 sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame)+sizeof(TimestampedInspvaaFrame),
307 &d,sizeof(TimestampedInscovFrame));
308
309 if (isRecording()) {
310 size_t dataSize = sizeof(inscovFrame_);
311 inscovFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
312 reinterpret_cast<const char *>(&inscovFrame_), dataSize);
313 // FIXED: was inscovFile_ instead of inscovFrame_ given as buffer
314 }
315 if (mVerbose) {
316 cout << "[SPAN-INSCOV] CovXX: " << inscovFrame_.PosCov[0][0] <<
317 " CovYY: " << inscovFrame_.PosCov[1][1] <<
318 " CovZZ: "<< inscovFrame_.PosCov[2][2] << endl;
319 }
320
321 //ShMem refresh
322 mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
323 +sizeof(TimestampedRawimusaFrame)
324 +sizeof(TimestampedInspvaaFrame)
325 +sizeof(TimestampedInscovFrame));
326 break;
327 default:
328 break;
329 }
330 }
331 }
332
333 type = -1;
334 newFrameToDecode = false;
335 frameToDecode = "";
336 }
337
338 memcpy(mAllFramesBuffer, allFramesBuffer, AllFramesSize);
339
340 delete[] allFramesBuffer;
341
342}
343
344/************************************************************************/
345/* Method to stop the thread */
346/************************************************************************/
347void CPTComponent::stopActivity()
348{
349 serialPort->THREAD_ALIVE = false;
350 if (!serialPort->wait(2000)) {
351 serialPort->terminate();
352 qDebug("The Win32SerialPort thread blocks anormally, it has been killed !!");
353 }
354 if ( !serialPort->closePort() )
355 qDebug("Failed to close the port");
356 else
357 qDebug("The port is closed");
358 delete serialPort;
359 serialPort = NULL;
360
361 if (isRecording()) {
362 bestgpsposaFile_.close();
363 rawimuFile_.close();
364 inspvaFile_.close();
365 inscovFile_.close();
366 raws8hdFile.close();
367 dataFile_.close();
368 }
369}
370
371/************************************************************************/
372/* Method to start the thread */
373/************************************************************************/
374void CPTComponent::startActivity()
375{
376 type = -1;
377
378#if WIN32
379 serialPort = new Win32SerialPort(portName.toLatin1());
380 // Asynchrone
381 serialPort->setMode(FILE_FLAG_OVERLAPPED);
382 // Synchrone
383 //serialPort->setMode(0);
384#else
385 serialPort = new PosixSerialPort(portName.toLatin1());
386#endif
387
388 if (!serialPort->openPort(portName.toLatin1())) {
389 LOG_ERROR("cannot open the port " << portName);
390 LOG_INFO("cannot start SPAN CPT Component '" << name() << "'");
391 return;
392 }
393
394 serialPort->configurePort(xmlParameters().getProperty("baudrate").toLong(),
395 xmlParameters().getProperty("bytesize").toUInt(),
396 xmlParameters().getProperty("parity").toShort(),
397 xmlParameters().getProperty("stopbits").toUInt() - 1);
398
399 if (isRecording()) {
400 bestgpsposaFile_.open("SPAN_bestgpsposa.dbt", WriteMode, TRAME_BESTGPSPOSA, sizeof(trame_bestgpsposa));
401 rawimuFile_.open("SPAN_rawimusa.dbt", WriteMode, TRAME_RAWIMUSA, sizeof(trame_rawimusa));
402 inspvaFile_.open("SPAN_inspvaa.dbt", WriteMode, TRAME_INSPVAA, sizeof(trame_inspvaa));
403 inscovFile_.open("SPAN_inscov.dbt", WriteMode, TRAME_INSCOV, sizeof(trame_inscov));
404
405 // raw data file: Dbt header + binary data stream
406 raws8hdFile.open((name() + ".dbt").toStdString(), WriteMode, STREAM8POSITION, sizeof(Stream8Position));
407 // FIXME: use ofstream
408 // open the file with C function to be sure that it will exist
409 FILE * stream = NULL;
410 if ( ( stream = fopen((name() + ".utc").toLatin1().data(),"a+") ) == NULL ) {
411 LOG_FATAL("Error while opening the stream utc file\n press a ENTER to exit");
412 getchar();
413 ::exit(-1);
414 } else {
415 fclose(stream);
416 }
417
418 dataFile_.open((name() + ".utc").toLatin1().data(), ios_base::out|ios_base::binary|ios_base::app);
419 if (!dataFile_)
420 {
421 printf("Error while opening the file alasca_data.utc\n press a ENTER to exit\n");
422 getchar();
423 ::exit(0);
424 }
425 }
426
427 if (!connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(processing(int)))) {
428 LOG_WARN("cannot connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)");
429 }
430 LOG_INFO("started component '" << name() << "'");
431
432 position2DENUOutput_ = getTypedOutput<QPointF, CPTComponent>("position2DENU");
433
434
435 serialPort->THREAD_ALIVE = true;
436 serialPort->start();
437
438}
439
440int CPTComponent::analyzeFrame()
441{
442 // SOF = start of frame
443 // EOF = end of frame
444
445 // PPS case
446 //if (currentDataFrame == "PPS")
447 //{
448 // frameToDecode = currentDataFrame;
449 // newFrameToDecode = true;
450 // timeOfFrameToDecode = currentRoadtime;
451 // timerangeOfFrameToDecode = currentTimerange;
452 // return 1;
453 //}
454
455 currentDataFrame_ = restOfFrame + currentDataFrame_;
456 restOfFrame = "";
457
458 if (currentDataFrame_.indexOf(QRegExp("[$#%]"),0)!=-1) {
459 sofIdx_ = currentDataFrame_.indexOf(QRegExp("[$#%]"),0);
460 startOfFrame = true;
461 timeOfFrameToDecode = currentRoadtime_;
462
463 if (currentDataFrame_.indexOf("\n",sofIdx_)!=-1) {
464 eofIdx_ = currentDataFrame_.indexOf("\n",sofIdx_);
465 endOfFrame = true;
466 timerangeOfFrameToDecode = currentTimerange_;
467 }
468 else
469 restOfFrame = currentDataFrame_;
470 }
471 else
472 restOfFrame = currentDataFrame_;
473
474 if ( (startOfFrame) && (endOfFrame) )
475 {
476 newFrameToDecode = true;
477 for (int i=sofIdx_;i<eofIdx_;i++)
478 frameToDecode +=currentDataFrame_.at(i);
479 for (int j=eofIdx_+1; j<currentDataFrame_.size(); j++)
480 restOfFrame +=currentDataFrame_.at(j);
481 if (mVerbose) {
482 LOG_DEBUG("[Frame:] " << frameToDecode);
483 }
484 }
485 startOfFrame = false;
486 endOfFrame = false;
487 sofIdx_ =0;
488 eofIdx_ =0;
489
490 return 1;
491}
492
493int CPTComponent::frameType(const QString frame)
494{
495 if (( frame[0] == 'P' ) && (frame[1] == 'P')&&( frame[2] == 'S' )) return SIGNAL_PPS ;
496 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'G')&&(frame[5] == 'A')) return TRAME_GGA_DBL ;
497 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'A')) return TRAME_GSA ;
498 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'H' ) && (frame[4] == 'D')&&(frame[5] == 'T')) return TRAME_HDT ;
499 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'T')) return TRAME_GST ;
500 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'V')) return TRAME_GSV ;
501 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'M')&&(frame[5] == 'C')) return TRAME_RMC ;
502 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'O')&&(frame[5] == 'T')) return TRAME_ROT ;
503 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'V' ) && (frame[4] == 'T')&&(frame[5] == 'G')) return TRAME_VTG ;
504 if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'Z' ) && (frame[4] == 'D')&&(frame[5] == 'A')) return TRAME_ZDA ;
505 if ((frame[0]== '#')||(frame[0]== '%')) { // Span CPT frames
506 if ((frame.contains("BESTGPSPOSA", Qt::CaseSensitive)))
507 return TRAME_BESTGPSPOSA;
508 else if ((frame.contains("RAWIMUSA", Qt::CaseSensitive)))
509 return TRAME_RAWIMUSA;
510 else if ((frame.contains("INSPVAA", Qt::CaseSensitive))||(frame.contains("INSPVASA", Qt::CaseSensitive)))
511 return TRAME_INSPVAA;
512 else if ((frame.contains("INSCOVA", Qt::CaseSensitive))||(frame.contains("INSCOVSA", Qt::CaseSensitive)))
513 return TRAME_INSCOV;
514 else
515 return UNKNOWN_NMEA_FRAME;
516 }
517 return UNKNOWN_NMEA_FRAME;
518}
519
520int CPTComponent::decodeFrame(int type)
521{
522 double lat_rad = 0, lon_rad = 0;
523 int indexGSV = 0;
524 int indexGSA = 0;
525 //int pps = 1; // dummy value for pps isRecording()
526
527 SENTENCE sentence;
528 sentence.Sentence = frameToDecode;
529
530 QStringList stringList;
531 QString header;
532 QString data;
533
534 switch(type) {
535 case UNKNOWN_NMEA_FRAME:
536 LOG_ERROR("received unknown frame");
537 break;
538
539 case SIGNAL_PPS:
540 // TODO
541 LOG_WARN("unimplemented");
542 break;
543
544 case TRAME_GGA_DBL:
545 if (sentence.Sentence.contains("PPS", Qt::CaseSensitive)) {
546 LOG_ERROR("Almanac error on SPAN CPT");
547 return -1;
548 } else {
549 if (!nmea0183_.Gga.Parse(sentence)) {
550 LOG_ERROR("cannot parse the frame " << nmea0183_.Gga.ErrorMessage);
551 }
552 lat_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Latitude.GetDecimalDegrees());
553 lon_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Longitude.GetDecimalDegrees());
554 ggaFrame_.H = nmea0183_.Gga.Time.time().hour();
555 ggaFrame_.Mi = nmea0183_.Gga.Time.time().minute();
556 ggaFrame_.S = nmea0183_.Gga.Time.time().second();
557 ggaFrame_.Ms = nmea0183_.Gga.Time.time().msec();
558 ggaFrame_.lon = lon_rad;
559 ggaFrame_.lat = lat_rad;
560 ggaFrame_.ind_qualite = nmea0183_.Gga.GPSQuality;
561 ggaFrame_.nb_sat = nmea0183_.Gga.NumberOfSatellitesInUse;
562 ggaFrame_.hdop = nmea0183_.Gga.HorizontalDilutionOfPrecision;
563 ggaFrame_.alt_msl = nmea0183_.Gga.AntennaAltitudeMeters;
564 ggaFrame_.d_geoidal = nmea0183_.Gga.GeoidalSeparationMeters;
565 ggaFrame_.age = nmea0183_.Gga.AgeOfDifferentialGPSDataSeconds;
566 ggaFrame_.dir_lat = ( (nmea0183_.Gga.Position.Latitude.Northing == North) ? 'N' : 'S' );
567 ggaFrame_.dir_lon = ( (nmea0183_.Gga.Position.Longitude.Easting == East) ? 'E' : 'W' );
568 ggaFrame_.ref_station_ID = nmea0183_.Gga.DifferentialReferenceStationID;
569 }
570 break;
571
572 case TRAME_GSA:
573 if (!nmea0183_.Gsa.Parse(sentence))
574 qWarning("Failed to parse the frame %s\n", nmea0183_.Gsa.ErrorMessage.toLatin1().data());
575 gsaFrame_.mode_select = ((nmea0183_.Gsa.OperatingMode == GSA::Manual) ? 'M' : 'A');
576 gsaFrame_.mode_result = 0;
577 if (nmea0183_.Gsa.FixMode == GSA::FixUnavailable)
578 gsaFrame_.mode_result = 1;
579 if (nmea0183_.Gsa.FixMode == GSA::TwoDimensional)
580 gsaFrame_.mode_result = 2;
581 if (nmea0183_.Gsa.FixMode == GSA::ThreeDimensional)
582 gsaFrame_.mode_result = 3;
583 for (indexGSA = 0 ; indexGSA<12 ; indexGSA++)
584 gsaFrame_.SV_PRN[indexGSA] = nmea0183_.Gsa.SatelliteNumber[indexGSA];
585 gsaFrame_.pdop = nmea0183_.Gsa.PDOP;
586 gsaFrame_.hdop = nmea0183_.Gsa.HDOP;
587 gsaFrame_.vdop = nmea0183_.Gsa.VDOP;
588
589 break;
590
591 case TRAME_GST:
592 if (!nmea0183_.Gst.Parse( sentence ))
593 qWarning("Failed to parse the frame %s\n",nmea0183_.Gst.ErrorMessage.toLatin1().data());
594 gstFrame_.rms = nmea0183_.Gst.RMSvalue;
595 gstFrame_.a = nmea0183_.Gst.ErrorEllipseMajor;
596 gstFrame_.b = nmea0183_.Gst.ErrorEllipseMinor;
597 gstFrame_.phi = nmea0183_.Gst.ErrorEllipseOrientation;
598 gstFrame_.sigma_lat = nmea0183_.Gst.LatitudeError;
599 gstFrame_.sigma_lon = nmea0183_.Gst.LongitudeError;
600 gstFrame_.sigma_alt = nmea0183_.Gst.HeightError;
601 gstFrame_.H = nmea0183_.Gst.Time.time().hour();
602 gstFrame_.Mi = nmea0183_.Gst.Time.time().minute();
603 gstFrame_.S = nmea0183_.Gst.Time.time().second();
604 gstFrame_.Ms = nmea0183_.Gst.Time.time().msec();
605
606 break;
607
608 case TRAME_GSV:
609 indexGSV = 0;
610 if (!nmea0183_.Gsv.Parse( sentence ))
611 qWarning("Failed to parse the frame %s\n",nmea0183_.Gsv.ErrorMessage.toLatin1().data());
612 // it's a new frame, reset stored value in case of the number of satellites
613 // in view has decreased
614 if (nmea0183_.Gsv.message_number == 1)
615 {
616 while (indexGSV < 36)
617 {
618 gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = 0;
619 gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = 0;
620 gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = 0;
621 gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = 0;
622 indexGSV++;
623 }
624 }
625 gsvFrame_.NumberOfSatellites = nmea0183_.Gsv.NumberOfSatellites;
626 gsvFrame_.Totalmessages = nmea0183_.Gsv.Totalmessages;
627
628 for ( indexGSV=4*(nmea0183_.Gsv.message_number-1); indexGSV<=(4*nmea0183_.Gsv.message_number)-1; indexGSV++ )
629 {
630 gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SatelliteNumber;
631 gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].ElevationDegrees;
632 gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].AzimuthDegreesTrue;
633 gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SignalToNoiseRatio;
634 }
635
636 break;
637
638 case TRAME_HDT:
639 if (!nmea0183_.Hdt.Parse( sentence ))
640 qWarning("Failed to parse the frame %s\n",nmea0183_.Hdt.ErrorMessage.toLatin1().data());
641 hdtFrame_.DegreesTrue = nmea0183_.Hdt.DegreesTrue;
642
643 break;
644
645 case TRAME_RMC:
646 if (!nmea0183_.Rmc.Parse( sentence ))
647 qWarning("Failed to parse the frame %s\n",nmea0183_.Rmc.ErrorMessage.toLatin1().data());
648 rmcFrame_.H = nmea0183_.Rmc.Time.time().hour();
649 rmcFrame_.Mi = nmea0183_.Rmc.Time.time().minute();
650 rmcFrame_.S = nmea0183_.Rmc.Time.time().second();
651 rmcFrame_.Ms = nmea0183_.Rmc.Time.time().msec();
652 rmcFrame_.AA = nmea0183_.Rmc.Time.date().year();
653 rmcFrame_.MM = nmea0183_.Rmc.Time.date().month();
654 rmcFrame_.JJ = nmea0183_.Rmc.Time.date().day();
655 rmcFrame_.lat = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Latitude.GetDecimalDegrees());
656 rmcFrame_.dir_lat = ( (nmea0183_.Rmc.Position.Latitude.Northing == North) ? 'N' : 'S');
657 rmcFrame_.lon = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Longitude.GetDecimalDegrees());
658 rmcFrame_.dir_lon = ( (nmea0183_.Rmc.Position.Longitude.Easting == East) ? 'E' : 'W' );
659 rmcFrame_.magnet_var = nmea0183_.Rmc.MagneticVariation;
660 rmcFrame_.dir_magnet_var = ( (nmea0183_.Rmc.MagneticVariationDirection == East) ? 'E' : 'W');
661 rmcFrame_.mode = -1;
662 if (nmea0183_.Rmc.ModeIndication == "A")
663 rmcFrame_.mode = 1;
664 if (nmea0183_.Rmc.ModeIndication == "D")
665 rmcFrame_.mode = 2;
666 if (nmea0183_.Rmc.ModeIndication == "N")
667 rmcFrame_.mode = 0;
668 rmcFrame_.track_true_north = nmea0183_.Rmc.TrackMadeGoodDegreesTrue;
669 rmcFrame_.valid_data = ( (nmea0183_.Rmc.IsDataValid == True) ? 1 : 0 );
670 rmcFrame_.vitesse = nmea0183_.Rmc.SpeedOverGroundKnots * 1852.0 / 3600.0; // 1 knot = 1852 m/h
671
672 break;
673
674 case TRAME_ROT:
675 if (!nmea0183_.Rot.Parse( sentence ))
676 qWarning("Failed to parse the frame %s\n",nmea0183_.Rot.ErrorMessage.toLatin1().data());
677 rotFrame_.RateOfTurn = nmea0183_.Rot.RateOfTurn;
678 rotFrame_.valid_data = ( (nmea0183_.Rot.IsDataValid == True) ? 1 : 0 );
679
680 break;
681
682 case TRAME_VTG:
683 if (!nmea0183_.Vtg.Parse( sentence ))
684 qWarning("Failed to parse the frame %s\n",nmea0183_.Vtg.ErrorMessage.toLatin1().data());
685 vtgFrame_.v = nmea0183_.Vtg.SpeedKilometersPerHour;
686 vtgFrame_.track_true_north = nmea0183_.Vtg.TrackDegreesTrue;
687 vtgFrame_.track_magnet_north = nmea0183_.Vtg.TrackDegreesMagnetic;
688
689 break;
690
691 case TRAME_ZDA:
692 if (!nmea0183_.Zda.Parse( sentence ))
693 qWarning("Failed to parse the frame %s\n",nmea0183_.Zda.ErrorMessage.toLatin1().data());
694 zdaFrame_.H = nmea0183_.Zda.Time.time().hour();
695 zdaFrame_.Mi = nmea0183_.Zda.Time.time().minute();
696 zdaFrame_.S = nmea0183_.Zda.Time.time().second();
697 zdaFrame_.Ms = nmea0183_.Zda.Time.time().msec();
698 zdaFrame_.AA = nmea0183_.Zda.Time.date().year();
699 zdaFrame_.MM = nmea0183_.Zda.Time.date().month();
700 zdaFrame_.JJ = nmea0183_.Zda.Time.date().day();
701 zdaFrame_.H_offset = nmea0183_.Zda.LocalHourDeviation;
702 zdaFrame_.Mi_offset = nmea0183_.Zda.LocalMinutesDeviation;
703
704 break;
705
706 case TRAME_BESTGPSPOSA:
707 stringList = sentence.Sentence.split(";");
708 if (stringList.size()==2) {
709 header = stringList.at(0);
710 data = stringList.at(1);
711
712 if (mVerbose) {
713 LOG_DEBUG("Header " << stringList.at(0));
714 LOG_DEBUG("Data " << stringList.at(1));
715 }
716
717 if (parseCPTbestgpsposa(data)) {
718 if (mVerbose) {
719 LOG_DEBUG("parsed TRAME_BESTGPSPOSA");
720 }
721 }
722 }
723 break;
724
725 case TRAME_RAWIMUSA:
726 stringList = sentence.Sentence.split(";");
727 LOG_DEBUG(sentence.Sentence);
728 if (stringList.size()==2) {
729 header = stringList.at(0);
730 data = stringList.at(1);
731
732 if (mVerbose) {
733 LOG_DEBUG("Header " << stringList.at(0));
734 LOG_DEBUG("Data " << stringList.at(1));
735 }
736
737 if (parseCPTrawimusa(data)) {
738 if (mVerbose) {
739 LOG_DEBUG("parsed TRAME_RAWIMUSA");
740 }
741 }
742 }
743 break;
744 case TRAME_INSPVAA:
745 stringList = sentence.Sentence.split(";");
746 LOG_DEBUG(sentence.Sentence);
747 if (stringList.size()==2) {
748 header = stringList.at(0);
749 data = stringList.at(1);
750 if (mVerbose) {
751 LOG_DEBUG("Header " << stringList.at(0));
752 LOG_DEBUG("Data " << stringList.at(1));
753 }
754 if (parseCPTinspvaa(data)) {
755 if (mVerbose) {
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 if (mVerbose) {
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}
1104
1105} // namespace pacpus
Note: See TracBrowser for help on using the repository browser.