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

Last change on this file since 80 was 78, checked in by DHERBOMEZ Gérald, 10 years ago
  • Usage of the new interface of win32serialport
  • Correction of a bug in frames initialization


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