1 | // *********************************************************************
|
---|
2 | // created: 20011/06/28 - 18:13
|
---|
3 | // filename: cptComponent.cpp
|
---|
4 | //
|
---|
5 | // author: Sergio Rodriguez
|
---|
6 | //
|
---|
7 | // version: $Id: $
|
---|
8 | //
|
---|
9 | // purpose: Acquires SPAN CPT data
|
---|
10 | //
|
---|
11 | // *********************************************************************
|
---|
12 |
|
---|
13 | #include "CPTComponent.h"
|
---|
14 |
|
---|
15 | #include <iostream>
|
---|
16 |
|
---|
17 | #include "kernel/ComponentFactory.h"
|
---|
18 | #include "kernel/DbiteFileTypes.h"
|
---|
19 | #include "kernel/Log.h"
|
---|
20 | #include "PacpusTools/geodesie.h"
|
---|
21 | #include "PacpusTools/ShMem.h"
|
---|
22 |
|
---|
23 | using namespace pacpus;
|
---|
24 | using namespace std;
|
---|
25 |
|
---|
26 | /// Construct the factory
|
---|
27 | static ComponentFactory<CPTComponent> sFactory("CPTComponent");
|
---|
28 |
|
---|
29 | static const size_t AllFramesSize = sizeof(TimestampedBestgpsposaFrame)
|
---|
30 | + sizeof(TimestampedRawimusaFrame)
|
---|
31 | + sizeof(TimestampedInspvaaFrame)
|
---|
32 | + sizeof(TimestampedInscovFrame)
|
---|
33 | ;
|
---|
34 |
|
---|
35 | DECLARE_STATIC_LOGGER("pacpus.base.CPTComponent");
|
---|
36 |
|
---|
37 | void CPTComponent::addInputOutput()
|
---|
38 | {
|
---|
39 | ADD_OUTPUT("bestposegpsposa",CPTComponent,TimestampedBestgpsposaFrame);
|
---|
40 | ADD_OUTPUT("rawimusa",CPTComponent,TimestampedRawimusaFrame);
|
---|
41 | ADD_OUTPUT("inspvaaa",CPTComponent,TimestampedInspvaaFrame);
|
---|
42 | ADD_OUTPUT("inscov",CPTComponent,TimestampedInscovFrame);
|
---|
43 | ADD_OUTPUT("genericpose",CPTComponent,Pose3D);
|
---|
44 | }
|
---|
45 |
|
---|
46 |
|
---|
47 | //////////////////////////////////////////////////////////////////////////
|
---|
48 | /// Constructor of CPTComponent class
|
---|
49 | CPTComponent::CPTComponent(QString name)
|
---|
50 | : ComponentBase(name)
|
---|
51 | {
|
---|
52 | LOG_INFO("sizeof(TimestampedBestgpsposaFrame)=" << sizeof(TimestampedBestgpsposaFrame));
|
---|
53 | LOG_INFO("sizeof(TimestampedRawimusaFrame)=" << sizeof(TimestampedRawimusaFrame));
|
---|
54 | LOG_INFO("sizeof(TimestampedInspvaaFrame)=" << sizeof(TimestampedInspvaaFrame));
|
---|
55 | LOG_INFO("sizeof(TimestampedInscovFrame)=" << sizeof(TimestampedInscovFrame));
|
---|
56 |
|
---|
57 | mVerbose = false;
|
---|
58 | recording = false;
|
---|
59 | enuRef_ = false;
|
---|
60 | LAT_REF =0.0;
|
---|
61 | LON_REF =0.0;
|
---|
62 | HE_REF =0.0;
|
---|
63 |
|
---|
64 | currentRoadtime_ = 0;
|
---|
65 | currentTimerange_ = 0;
|
---|
66 |
|
---|
67 | //decoding
|
---|
68 | currentDataFrame ="";
|
---|
69 | currentRoadtime = 0;
|
---|
70 | currentTimerange = 0;
|
---|
71 |
|
---|
72 | frameToDecode = "";
|
---|
73 | timeOfFrameToDecode = 0;
|
---|
74 | timerangeOfFrameToDecode = 0;
|
---|
75 |
|
---|
76 | restOfFrame = "";
|
---|
77 |
|
---|
78 | newFrameToDecode = false;
|
---|
79 | startOfFrame = false;
|
---|
80 | sofIdx_ =0;
|
---|
81 | endOfFrame = false;
|
---|
82 | eofIdx_ =0;
|
---|
83 |
|
---|
84 | //ShMems allocation
|
---|
85 | mShMem = new ShMem("SPAN_FRAMES", AllFramesSize);
|
---|
86 | mAllFramesBuffer = new char[AllFramesSize];
|
---|
87 | addInputOutput();
|
---|
88 | }
|
---|
89 |
|
---|
90 | /// Destructor of the CPTComponent class
|
---|
91 | CPTComponent::~CPTComponent()
|
---|
92 | {
|
---|
93 | delete mShMem;
|
---|
94 | delete[] mAllFramesBuffer;
|
---|
95 | }
|
---|
96 |
|
---|
97 | ComponentBase::COMPONENT_CONFIGURATION CPTComponent::configureComponent(XmlComponentConfig /*config*/)
|
---|
98 | {
|
---|
99 | setPortCOM( param.getProperty("port").toLatin1() );
|
---|
100 |
|
---|
101 | if (!param.getProperty("recording").isNull()) {
|
---|
102 | recording = (param.getProperty("recording") == "true" ? true : false);
|
---|
103 | }
|
---|
104 | // if (!param.getProperty("verbose").isNull()) {
|
---|
105 | // mVerbose = (param.getProperty("verbose") == "true" ? true : false);
|
---|
106 | // }
|
---|
107 | if (!param.getProperty("ENUref").isNull()) {
|
---|
108 | enuRef_ = (param.getProperty("ENUref") == "true" ? true : false);
|
---|
109 | if (enuRef_) {
|
---|
110 | LAT_REF = param.getProperty("LatRef").toDouble();
|
---|
111 | LON_REF = param.getProperty("LonRef").toDouble();
|
---|
112 | HE_REF = param.getProperty("HeRef").toDouble();
|
---|
113 | LOG_DEBUG("Reference Saint Mande:: Lat: " << LAT_REF << ", Lon: " << LON_REF << " Height: " << HE_REF);
|
---|
114 | }
|
---|
115 | }
|
---|
116 | return ComponentBase::CONFIGURED_OK;
|
---|
117 | }
|
---|
118 |
|
---|
119 | void CPTComponent::setPortCOM(const char * port)
|
---|
120 | {
|
---|
121 | portName = port;
|
---|
122 | }
|
---|
123 |
|
---|
124 | /// Log function
|
---|
125 | void CPTComponent::processing(int /*i*/)
|
---|
126 | {
|
---|
127 | if (!serialPort) {
|
---|
128 | LOG_WARN("no serial port");
|
---|
129 | return;
|
---|
130 | }
|
---|
131 |
|
---|
132 | //ShMem data variables
|
---|
133 | TimestampedBestgpsposaFrame a;
|
---|
134 | TimestampedRawimusaFrame b;
|
---|
135 | TimestampedInspvaaFrame c;
|
---|
136 | TimestampedInscovFrame d;
|
---|
137 |
|
---|
138 | //ShMem buffer allocation
|
---|
139 | char * allFramesBuffer = new char[AllFramesSize];
|
---|
140 |
|
---|
141 | //ShMem buffer initialization
|
---|
142 | initBuffer(&a, &b, &c, &d);
|
---|
143 | memcpy(allFramesBuffer,
|
---|
144 | &a, sizeof(TimestampedBestgpsposaFrame));
|
---|
145 | memcpy(allFramesBuffer
|
---|
146 | + sizeof(TimestampedBestgpsposaFrame),
|
---|
147 | &b,sizeof(TimestampedRawimusaFrame));
|
---|
148 | memcpy(allFramesBuffer
|
---|
149 | + sizeof(TimestampedBestgpsposaFrame) + sizeof(TimestampedRawimusaFrame),
|
---|
150 | &c, sizeof(TimestampedInspvaaFrame));
|
---|
151 | memcpy(allFramesBuffer
|
---|
152 | + sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame)+sizeof(TimestampedInspvaaFrame),
|
---|
153 | &d, sizeof(TimestampedInscovFrame));
|
---|
154 |
|
---|
155 | memcpy(allFramesBuffer, mAllFramesBuffer, AllFramesSize);
|
---|
156 |
|
---|
157 | // get the frame and remove it in the list
|
---|
158 | FRAME * currentFrame = serialPort->firstFrame();
|
---|
159 |
|
---|
160 | char * currentDataFrame = new char[currentFrame->length];
|
---|
161 | memcpy(currentDataFrame, currentFrame->data, currentFrame->length);
|
---|
162 | currentDataFrameLength_ = currentFrame->length;
|
---|
163 | currentRoadtime_ = currentFrame->t;
|
---|
164 |
|
---|
165 | serialPort->removeFirstFrame();
|
---|
166 |
|
---|
167 | setState(ComponentBase::MONITOR_OK);
|
---|
168 |
|
---|
169 | if (recording) {
|
---|
170 | s8Data_.dataPos = dataFile_.tellp();
|
---|
171 | s8Data_.length = currentDataFrameLength_;
|
---|
172 |
|
---|
173 | LOG_DEBUG("data:" << currentDataFrame << "addr:" << s8Data_.dataPos << "l:" << s8Data_.length);
|
---|
174 |
|
---|
175 | size_t dataSize = sizeof(Stream8Position);
|
---|
176 | raws8hdFile.writeRecord(currentRoadtime_, 0, reinterpret_cast<const char *>(&s8Data_), dataSize);
|
---|
177 | dataFile_.write(reinterpret_cast<char*>(currentDataFrame), currentDataFrameLength_);
|
---|
178 | }
|
---|
179 |
|
---|
180 | // Decoding
|
---|
181 | char * buffer = new char[currentDataFrameLength_ + 1];
|
---|
182 | memcpy(buffer, currentDataFrame, currentDataFrameLength_);
|
---|
183 | buffer[currentDataFrameLength_] = '\0'; // add a \0 to convert for the conversion in QString
|
---|
184 |
|
---|
185 | currentDataFrame = buffer;
|
---|
186 | currentRoadtime = currentRoadtime_;
|
---|
187 | currentTimerange = currentRoadtime_;
|
---|
188 |
|
---|
189 | analyzeFrame();
|
---|
190 |
|
---|
191 | if (newFrameToDecode) {
|
---|
192 | LOG_DEBUG("got new frame to decode");
|
---|
193 |
|
---|
194 | type = frameType(frameToDecode);
|
---|
195 | LOG_DEBUG("frame type=" << type);
|
---|
196 | if (-1 != type) {
|
---|
197 | if (-1 == decodeFrame(type)) {
|
---|
198 | LOG_WARN("cannot decode the dataframe");
|
---|
199 | } else {
|
---|
200 | switch(type) {
|
---|
201 | case TRAME_GGA_DBL:
|
---|
202 | if (mVerbose) {
|
---|
203 | cout << "[SPAN-GGA] Lat: " <<
|
---|
204 | ggaFrame_.lat<<
|
---|
205 | " Lon: " << ggaFrame_.lon<<
|
---|
206 | " Hgt: " << ggaFrame_.alt_msl <<
|
---|
207 | " Quality: " << ggaFrame_.ind_qualite << "\n";}
|
---|
208 | break;
|
---|
209 |
|
---|
210 | case TRAME_BESTGPSPOSA:
|
---|
211 | //ShMem buffer data copy
|
---|
212 | memcpy(&a.frame, &bestgpsposaFrame_, sizeof(trame_bestgpsposa));
|
---|
213 | a.time=timeOfFrameToDecode;
|
---|
214 | a.timerange=timerangeOfFrameToDecode;
|
---|
215 | memcpy(allFramesBuffer, &a, sizeof(TimestampedBestgpsposaFrame));
|
---|
216 |
|
---|
217 | if (recording) {
|
---|
218 | size_t dataSize = sizeof(bestgpsposaFrame_);
|
---|
219 | bestgpsposaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
|
---|
220 | reinterpret_cast<const char *>(&bestgpsposaFrame_), dataSize);
|
---|
221 | }
|
---|
222 | LOG_DEBUG("[SPAN-BESTGPSPOSA]" << "\t"
|
---|
223 | << "Lat=" << bestgpsposaFrame_.Lat << "\t"
|
---|
224 | << "Lon=" << bestgpsposaFrame_.Lon << "\t"
|
---|
225 | << "Hgt=" << bestgpsposaFrame_.Hgt << "\t"
|
---|
226 | << "E=" << bestgpsposaFrame_.e << "\t"
|
---|
227 | << "N=" << bestgpsposaFrame_.n << "\t"
|
---|
228 | << "U=" << bestgpsposaFrame_.u) ;
|
---|
229 |
|
---|
230 | mShMem->write(allFramesBuffer, AllFramesSize);
|
---|
231 |
|
---|
232 | GET_OUTPUT("bestposegpsposa",CPTComponent,TimestampedBestgpsposaFrame)->send(a);
|
---|
233 |
|
---|
234 | break;
|
---|
235 |
|
---|
236 | case TRAME_RAWIMUSA:
|
---|
237 |
|
---|
238 | //ShMem buffer data copy
|
---|
239 | memcpy(&b.frame, &rawimuFrame_, sizeof(trame_rawimusa));
|
---|
240 | b.time=timeOfFrameToDecode;
|
---|
241 | b.timerange=timerangeOfFrameToDecode;
|
---|
242 | memcpy(allFramesBuffer+
|
---|
243 | sizeof(TimestampedBestgpsposaFrame),&b,sizeof(TimestampedRawimusaFrame));
|
---|
244 |
|
---|
245 | if (recording) {
|
---|
246 | size_t dataSize = sizeof(rawimuFrame_);
|
---|
247 | rawimuFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
|
---|
248 | reinterpret_cast<const char *>(&rawimuFrame_), dataSize);
|
---|
249 | }
|
---|
250 | LOG_DEBUG("[SPAN-RAWIMUSA]");
|
---|
251 |
|
---|
252 |
|
---|
253 | //ShMem refresh
|
---|
254 | mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
|
---|
255 | +sizeof(TimestampedRawimusaFrame)
|
---|
256 | +sizeof(TimestampedInspvaaFrame)
|
---|
257 | +sizeof(TimestampedInscovFrame));
|
---|
258 | GET_OUTPUT("rawimusa",CPTComponent,TimestampedRawimusaFrame)->send(b);
|
---|
259 | break;
|
---|
260 | case TRAME_INSPVAA:
|
---|
261 |
|
---|
262 | //ShMem buffer data copy
|
---|
263 | memcpy(&c.frame, &inspvaFrame_, sizeof(trame_inspvaa));
|
---|
264 | c.time=timeOfFrameToDecode;
|
---|
265 | c.timerange=timerangeOfFrameToDecode;
|
---|
266 | memcpy(allFramesBuffer+
|
---|
267 | sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame),
|
---|
268 | &c,sizeof(TimestampedInspvaaFrame));
|
---|
269 |
|
---|
270 | if (recording) {
|
---|
271 | size_t dataSize = sizeof(inspvaFrame_);
|
---|
272 | inspvaFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
|
---|
273 | reinterpret_cast<const char *>(&inspvaFrame_), dataSize);
|
---|
274 | }
|
---|
275 | LOG_DEBUG("[SPAN-INSPVAA]:" << "\t"
|
---|
276 | << "Lat: " << inspvaFrame_.Lat << "\t"
|
---|
277 | << "Lon: " << inspvaFrame_.Lon << "\t"
|
---|
278 | << "Roll: "<< inspvaFrame_.Roll << "\t"
|
---|
279 | << "Pitch: " << inspvaFrame_.Azimuth << "\t"
|
---|
280 | << "E: " << inspvaFrame_.e << "\t"
|
---|
281 | << "N: " << inspvaFrame_.n << "\t"
|
---|
282 | << "U: " << inspvaFrame_.u);
|
---|
283 |
|
---|
284 | //ShMem refresh
|
---|
285 | mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
|
---|
286 | +sizeof(TimestampedRawimusaFrame)
|
---|
287 | +sizeof(TimestampedInspvaaFrame)
|
---|
288 | +sizeof(TimestampedInscovFrame));
|
---|
289 |
|
---|
290 | GET_OUTPUT("inspvaaa",CPTComponent,TimestampedInspvaaFrame)->send(c);
|
---|
291 |
|
---|
292 | genericEnuPose_.time = c.time;
|
---|
293 | genericEnuPose_.timerange = c.timerange;
|
---|
294 | genericEnuPose_.position = QVector3D(inspvaFrame_.e, inspvaFrame_.n, inspvaFrame_.u);
|
---|
295 | genericEnuPose_.angle = QVector3D((inspvaFrame_.Azimuth+90.0)*(M_PI/180.0),inspvaFrame_.Pitch*(M_PI/180.0),inspvaFrame_.Roll*(M_PI/180.0));
|
---|
296 |
|
---|
297 | // for (int i=0;i<3;i++) {
|
---|
298 | // for(int j=0;j<3;j++) {
|
---|
299 | // pose.posconv[i][j] = d.frame.PosCov[i][j];
|
---|
300 | // pose.angcov[i][j] = d.frame.AttCov[i][j];
|
---|
301 | // }
|
---|
302 | // }
|
---|
303 | GET_OUTPUT("genericpose",CPTComponent,Pose3D)->send(genericEnuPose_);
|
---|
304 |
|
---|
305 | break;
|
---|
306 | case TRAME_INSCOV:
|
---|
307 |
|
---|
308 | //ShMem data copy
|
---|
309 | memcpy(&d.frame, &inscovFrame_, sizeof(trame_inscov));
|
---|
310 | d.time=timeOfFrameToDecode;
|
---|
311 | d.timerange=timerangeOfFrameToDecode;
|
---|
312 | memcpy(allFramesBuffer+
|
---|
313 | sizeof(TimestampedBestgpsposaFrame)+sizeof(TimestampedRawimusaFrame)+sizeof(TimestampedInspvaaFrame),
|
---|
314 | &d,sizeof(TimestampedInscovFrame));
|
---|
315 |
|
---|
316 | if (recording) {
|
---|
317 | size_t dataSize = sizeof(inscovFrame_);
|
---|
318 | inscovFile_.writeRecord(timeOfFrameToDecode, timerangeOfFrameToDecode,
|
---|
319 | reinterpret_cast<const char *>(&inscovFrame_), dataSize);
|
---|
320 | // FIXED: was inscovFile_ instead of inscovFrame_ given as buffer
|
---|
321 | }
|
---|
322 | LOG_DEBUG("[SPAN-INSCOV] CovXX: " << inscovFrame_.PosCov[0][0] <<
|
---|
323 | " CovYY: " << inscovFrame_.PosCov[1][1] <<
|
---|
324 | " CovZZ: "<< inscovFrame_.PosCov[2][2]);
|
---|
325 |
|
---|
326 | //ShMem refresh
|
---|
327 | mShMem->write(allFramesBuffer, sizeof(TimestampedBestgpsposaFrame)
|
---|
328 | +sizeof(TimestampedRawimusaFrame)
|
---|
329 | +sizeof(TimestampedInspvaaFrame)
|
---|
330 | +sizeof(TimestampedInscovFrame));
|
---|
331 |
|
---|
332 | GET_OUTPUT("inscov",CPTComponent,TimestampedInscovFrame)->send(d);
|
---|
333 | break;
|
---|
334 | default:
|
---|
335 | break;
|
---|
336 | }
|
---|
337 | }
|
---|
338 | }
|
---|
339 |
|
---|
340 | type = -1;
|
---|
341 | newFrameToDecode = false;
|
---|
342 | frameToDecode = "";
|
---|
343 | }
|
---|
344 | delete[] buffer;
|
---|
345 | delete[] currentDataFrame;
|
---|
346 |
|
---|
347 | memcpy(mAllFramesBuffer, allFramesBuffer, AllFramesSize);
|
---|
348 |
|
---|
349 | delete[] allFramesBuffer;
|
---|
350 |
|
---|
351 | }
|
---|
352 |
|
---|
353 | /************************************************************************/
|
---|
354 | /* Method to stop the thread */
|
---|
355 | /************************************************************************/
|
---|
356 | void CPTComponent::stopActivity()
|
---|
357 | {
|
---|
358 | serialPort->THREAD_ALIVE = false;
|
---|
359 | if (!serialPort->wait(2000)) {
|
---|
360 | serialPort->terminate();
|
---|
361 | LOG_WARN("The SerialPort thread blocks anormally, it has been killed !!");
|
---|
362 | }
|
---|
363 | if ( !serialPort->closePort() ) {
|
---|
364 | LOG_ERROR("Failed to close the port");
|
---|
365 | } else
|
---|
366 | LOG_INFO("The port is closed");
|
---|
367 | delete serialPort;
|
---|
368 | serialPort = NULL;
|
---|
369 |
|
---|
370 | if (recording) {
|
---|
371 | bestgpsposaFile_.close();
|
---|
372 | rawimuFile_.close();
|
---|
373 | inspvaFile_.close();
|
---|
374 | inscovFile_.close();
|
---|
375 | raws8hdFile.close();
|
---|
376 | dataFile_.close();
|
---|
377 | }
|
---|
378 | }
|
---|
379 |
|
---|
380 | /************************************************************************/
|
---|
381 | /* Method to start the thread */
|
---|
382 | /************************************************************************/
|
---|
383 | void CPTComponent::startActivity()
|
---|
384 | {
|
---|
385 | type = -1;
|
---|
386 |
|
---|
387 | #if WIN32
|
---|
388 | serialPort = new Win32SerialPort(portName.toLatin1());
|
---|
389 | // Asynchrone
|
---|
390 | serialPort->setMode(FILE_FLAG_OVERLAPPED);
|
---|
391 | // Synchrone
|
---|
392 | //serialPort->setMode(0);
|
---|
393 | #else
|
---|
394 | serialPort = new PosixSerialPort(portName.toLatin1());
|
---|
395 | #endif
|
---|
396 |
|
---|
397 | if (!serialPort->openPort(portName.toLatin1())) {
|
---|
398 | LOG_ERROR("cannot open the port " << portName);
|
---|
399 | LOG_INFO("cannot start SPAN CPT Component '" << componentName << "'");
|
---|
400 | return;
|
---|
401 | }
|
---|
402 |
|
---|
403 | serialPort->configurePort(param.getProperty("baudrate").toLong(),
|
---|
404 | param.getProperty("bytesize").toUInt(),
|
---|
405 | param.getProperty("parity").toShort(),
|
---|
406 | param.getProperty("stopbits").toUInt() - 1);
|
---|
407 |
|
---|
408 | if (recording) {
|
---|
409 | bestgpsposaFile_.open("SPAN_bestgpsposa.dbt", WriteMode, TRAME_BESTGPSPOSA, sizeof(trame_bestgpsposa));
|
---|
410 | rawimuFile_.open("SPAN_rawimusa.dbt", WriteMode, TRAME_RAWIMUSA, sizeof(trame_rawimusa));
|
---|
411 | inspvaFile_.open("SPAN_inspvaa.dbt", WriteMode, TRAME_INSPVAA, sizeof(trame_inspvaa));
|
---|
412 | inscovFile_.open("SPAN_inscov.dbt", WriteMode, TRAME_INSCOV, sizeof(trame_inscov));
|
---|
413 |
|
---|
414 | // raw data file: Dbt header + binary data stream
|
---|
415 | raws8hdFile.open((ComponentBase::componentName + ".dbt").toStdString(), WriteMode, STREAM8POSITION, sizeof(Stream8Position));
|
---|
416 | // FIXME: use ofstream
|
---|
417 | // open the file with C function to be sure that it will exist
|
---|
418 | FILE * stream = NULL;
|
---|
419 | if ( ( stream = fopen((ComponentBase::componentName + ".utc").toLatin1().data(),"a+") ) == NULL ) {
|
---|
420 | LOG_FATAL("Error while opening the stream utc file\n press a ENTER to exit");
|
---|
421 | getchar();
|
---|
422 | ::exit(-1);
|
---|
423 | } else {
|
---|
424 | fclose(stream);
|
---|
425 | }
|
---|
426 |
|
---|
427 | dataFile_.open((ComponentBase::componentName + ".utc").toLatin1().data(), ios_base::out|ios_base::binary|ios_base::app);
|
---|
428 | if (!dataFile_)
|
---|
429 | {
|
---|
430 | LOG_ERROR("Error while opening the file alasca_data.utc\n press a ENTER to exit\n");
|
---|
431 | getchar();
|
---|
432 | ::exit(0);
|
---|
433 | }
|
---|
434 | }
|
---|
435 |
|
---|
436 | if (!connect(serialPort,SIGNAL(newDataAvailable(int)),this, SLOT(processing(int)))) {
|
---|
437 | LOG_WARN("cannot connect SIGNAL(newDataAvailable(int)) with SLOT(unlockProcessing(int)");
|
---|
438 | }
|
---|
439 | LOG_INFO("started component '" << componentName << "'");
|
---|
440 |
|
---|
441 | serialPort->THREAD_ALIVE = true;
|
---|
442 | serialPort->start();
|
---|
443 |
|
---|
444 | }
|
---|
445 |
|
---|
446 | int CPTComponent::analyzeFrame()
|
---|
447 | {
|
---|
448 | // SOF = start of frame
|
---|
449 | // EOF = end of frame
|
---|
450 |
|
---|
451 | // PPS case
|
---|
452 | //if (currentDataFrame == "PPS")
|
---|
453 | //{
|
---|
454 | // frameToDecode = currentDataFrame;
|
---|
455 | // newFrameToDecode = true;
|
---|
456 | // timeOfFrameToDecode = currentRoadtime;
|
---|
457 | // timerangeOfFrameToDecode = currentTimerange;
|
---|
458 | // return 1;
|
---|
459 | //}
|
---|
460 |
|
---|
461 | currentDataFrame = restOfFrame + currentDataFrame;
|
---|
462 | restOfFrame = "";
|
---|
463 |
|
---|
464 | if (currentDataFrame.indexOf(QRegExp("[$#%]"),0)!=-1) {
|
---|
465 | sofIdx_ = currentDataFrame.indexOf(QRegExp("[$#%]"),0);
|
---|
466 | startOfFrame = true;
|
---|
467 | timeOfFrameToDecode = currentRoadtime;
|
---|
468 |
|
---|
469 | if (currentDataFrame.indexOf("\n",sofIdx_)!=-1) {
|
---|
470 | eofIdx_ = currentDataFrame.indexOf("\n",sofIdx_);
|
---|
471 | endOfFrame = true;
|
---|
472 | timerangeOfFrameToDecode = currentTimerange;
|
---|
473 | }
|
---|
474 | else
|
---|
475 | restOfFrame = currentDataFrame;
|
---|
476 | }
|
---|
477 | else
|
---|
478 | restOfFrame = currentDataFrame;
|
---|
479 |
|
---|
480 | if ( (startOfFrame) && (endOfFrame) )
|
---|
481 | {
|
---|
482 | newFrameToDecode = true;
|
---|
483 | for (int i=sofIdx_;i<eofIdx_;i++)
|
---|
484 | frameToDecode +=currentDataFrame.at(i);
|
---|
485 | for (int j=eofIdx_+1; j<currentDataFrame.size(); j++)
|
---|
486 | restOfFrame +=currentDataFrame.at(j);
|
---|
487 | if (mVerbose) {
|
---|
488 | LOG_DEBUG("[Frame:] " << frameToDecode);
|
---|
489 | }
|
---|
490 | }
|
---|
491 | startOfFrame = false;
|
---|
492 | endOfFrame = false;
|
---|
493 | sofIdx_ =0;
|
---|
494 | eofIdx_ =0;
|
---|
495 |
|
---|
496 | return 1;
|
---|
497 | }
|
---|
498 |
|
---|
499 | int CPTComponent::frameType(const QString frame)
|
---|
500 | {
|
---|
501 | if (( frame[0] == 'P' ) && (frame[1] == 'P')&&( frame[2] == 'S' )) return SIGNAL_PPS ;
|
---|
502 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'G')&&(frame[5] == 'A')) return TRAME_GGA_DBL ;
|
---|
503 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'A')) return TRAME_GSA ;
|
---|
504 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'H' ) && (frame[4] == 'D')&&(frame[5] == 'T')) return TRAME_HDT ;
|
---|
505 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'T')) return TRAME_GST ;
|
---|
506 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'G' ) && (frame[4] == 'S')&&(frame[5] == 'V')) return TRAME_GSV ;
|
---|
507 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'M')&&(frame[5] == 'C')) return TRAME_RMC ;
|
---|
508 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'R' ) && (frame[4] == 'O')&&(frame[5] == 'T')) return TRAME_ROT ;
|
---|
509 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'V' ) && (frame[4] == 'T')&&(frame[5] == 'G')) return TRAME_VTG ;
|
---|
510 | if (( frame[1] == 'G' ) && (frame[2] == 'P')&&( frame[3] == 'Z' ) && (frame[4] == 'D')&&(frame[5] == 'A')) return TRAME_ZDA ;
|
---|
511 | if ((frame[0]== '#')||(frame[0]== '%')) { // Span CPT frames
|
---|
512 | if ((frame.contains("BESTGPSPOSA", Qt::CaseSensitive)))
|
---|
513 | return TRAME_BESTGPSPOSA;
|
---|
514 | else if ((frame.contains("RAWIMUSA", Qt::CaseSensitive)))
|
---|
515 | return TRAME_RAWIMUSA;
|
---|
516 | else if ((frame.contains("INSPVAA", Qt::CaseSensitive))||(frame.contains("INSPVASA", Qt::CaseSensitive)))
|
---|
517 | return TRAME_INSPVAA;
|
---|
518 | else if ((frame.contains("INSCOVA", Qt::CaseSensitive))||(frame.contains("INSCOVSA", Qt::CaseSensitive)))
|
---|
519 | return TRAME_INSCOV;
|
---|
520 | else
|
---|
521 | return UNKNOWN_NMEA_FRAME;
|
---|
522 | }
|
---|
523 | return UNKNOWN_NMEA_FRAME;
|
---|
524 | }
|
---|
525 |
|
---|
526 | int CPTComponent::decodeFrame(int type)
|
---|
527 | {
|
---|
528 | double lat_rad = 0, lon_rad = 0;
|
---|
529 | int indexGSV = 0;
|
---|
530 | int indexGSA = 0;
|
---|
531 | //int pps = 1; // dummy value for pps recording
|
---|
532 |
|
---|
533 | SENTENCE sentence;
|
---|
534 | sentence.Sentence = frameToDecode;
|
---|
535 |
|
---|
536 | QStringList stringList;
|
---|
537 | QString header;
|
---|
538 | QString data;
|
---|
539 |
|
---|
540 | switch(type) {
|
---|
541 | case UNKNOWN_NMEA_FRAME:
|
---|
542 | LOG_ERROR("received unknown frame");
|
---|
543 | break;
|
---|
544 |
|
---|
545 | case SIGNAL_PPS:
|
---|
546 | // TODO
|
---|
547 | LOG_WARN("unimplemented");
|
---|
548 | break;
|
---|
549 |
|
---|
550 | case TRAME_GGA_DBL:
|
---|
551 | if (sentence.Sentence.contains("PPS", Qt::CaseSensitive)) {
|
---|
552 | LOG_ERROR("Almanac error on SPAN CPT");
|
---|
553 | return -1;
|
---|
554 | } else {
|
---|
555 | if (!nmea0183_.Gga.Parse(sentence)) {
|
---|
556 | LOG_ERROR("cannot parse the frame " << nmea0183_.Gga.ErrorMessage);
|
---|
557 | }
|
---|
558 | lat_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Latitude.GetDecimalDegrees());
|
---|
559 | lon_rad = Geodesie::Deg2Rad(nmea0183_.Gga.Position.Longitude.GetDecimalDegrees());
|
---|
560 | ggaFrame_.H = nmea0183_.Gga.Time.time().hour();
|
---|
561 | ggaFrame_.Mi = nmea0183_.Gga.Time.time().minute();
|
---|
562 | ggaFrame_.S = nmea0183_.Gga.Time.time().second();
|
---|
563 | ggaFrame_.Ms = nmea0183_.Gga.Time.time().msec();
|
---|
564 | ggaFrame_.lon = lon_rad;
|
---|
565 | ggaFrame_.lat = lat_rad;
|
---|
566 | ggaFrame_.ind_qualite = nmea0183_.Gga.GPSQuality;
|
---|
567 | ggaFrame_.nb_sat = nmea0183_.Gga.NumberOfSatellitesInUse;
|
---|
568 | ggaFrame_.hdop = nmea0183_.Gga.HorizontalDilutionOfPrecision;
|
---|
569 | ggaFrame_.alt_msl = nmea0183_.Gga.AntennaAltitudeMeters;
|
---|
570 | ggaFrame_.d_geoidal = nmea0183_.Gga.GeoidalSeparationMeters;
|
---|
571 | ggaFrame_.age = nmea0183_.Gga.AgeOfDifferentialGPSDataSeconds;
|
---|
572 | ggaFrame_.dir_lat = ( (nmea0183_.Gga.Position.Latitude.Northing == North) ? 'N' : 'S' );
|
---|
573 | ggaFrame_.dir_lon = ( (nmea0183_.Gga.Position.Longitude.Easting == East) ? 'E' : 'W' );
|
---|
574 | ggaFrame_.ref_station_ID = nmea0183_.Gga.DifferentialReferenceStationID;
|
---|
575 | }
|
---|
576 | break;
|
---|
577 |
|
---|
578 | case TRAME_GSA:
|
---|
579 | if (!nmea0183_.Gsa.Parse(sentence))
|
---|
580 | LOG_WARN("Failed to parse the frame " << nmea0183_.Gsa.ErrorMessage.toLatin1().data());
|
---|
581 | gsaFrame_.mode_select = ((nmea0183_.Gsa.OperatingMode == GSA::Manual) ? 'M' : 'A');
|
---|
582 | gsaFrame_.mode_result = 0;
|
---|
583 | if (nmea0183_.Gsa.FixMode == GSA::FixUnavailable)
|
---|
584 | gsaFrame_.mode_result = 1;
|
---|
585 | if (nmea0183_.Gsa.FixMode == GSA::TwoDimensional)
|
---|
586 | gsaFrame_.mode_result = 2;
|
---|
587 | if (nmea0183_.Gsa.FixMode == GSA::ThreeDimensional)
|
---|
588 | gsaFrame_.mode_result = 3;
|
---|
589 | for (indexGSA = 0 ; indexGSA<12 ; indexGSA++)
|
---|
590 | gsaFrame_.SV_PRN[indexGSA] = nmea0183_.Gsa.SatelliteNumber[indexGSA];
|
---|
591 | gsaFrame_.pdop = nmea0183_.Gsa.PDOP;
|
---|
592 | gsaFrame_.hdop = nmea0183_.Gsa.HDOP;
|
---|
593 | gsaFrame_.vdop = nmea0183_.Gsa.VDOP;
|
---|
594 |
|
---|
595 | break;
|
---|
596 |
|
---|
597 | case TRAME_GST:
|
---|
598 | if (!nmea0183_.Gst.Parse( sentence ))
|
---|
599 | LOG_WARN("Failed to parse the frame " << nmea0183_.Gst.ErrorMessage.toLatin1().data());
|
---|
600 | gstFrame_.rms = nmea0183_.Gst.RMSvalue;
|
---|
601 | gstFrame_.a = nmea0183_.Gst.ErrorEllipseMajor;
|
---|
602 | gstFrame_.b = nmea0183_.Gst.ErrorEllipseMinor;
|
---|
603 | gstFrame_.phi = nmea0183_.Gst.ErrorEllipseOrientation;
|
---|
604 | gstFrame_.sigma_lat = nmea0183_.Gst.LatitudeError;
|
---|
605 | gstFrame_.sigma_lon = nmea0183_.Gst.LongitudeError;
|
---|
606 | gstFrame_.sigma_alt = nmea0183_.Gst.HeightError;
|
---|
607 | gstFrame_.H = nmea0183_.Gst.Time.time().hour();
|
---|
608 | gstFrame_.Mi = nmea0183_.Gst.Time.time().minute();
|
---|
609 | gstFrame_.S = nmea0183_.Gst.Time.time().second();
|
---|
610 | gstFrame_.Ms = nmea0183_.Gst.Time.time().msec();
|
---|
611 |
|
---|
612 | break;
|
---|
613 |
|
---|
614 | case TRAME_GSV:
|
---|
615 | indexGSV = 0;
|
---|
616 | if (!nmea0183_.Gsv.Parse( sentence ))
|
---|
617 | LOG_WARN("Failed to parse the frame " << nmea0183_.Gsv.ErrorMessage.toLatin1().data());
|
---|
618 | // it's a new frame, reset stored value in case of the number of satellites
|
---|
619 | // in view has decreased
|
---|
620 | if (nmea0183_.Gsv.message_number == 1)
|
---|
621 | {
|
---|
622 | while (indexGSV < 36)
|
---|
623 | {
|
---|
624 | gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = 0;
|
---|
625 | gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = 0;
|
---|
626 | gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = 0;
|
---|
627 | gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = 0;
|
---|
628 | indexGSV++;
|
---|
629 | }
|
---|
630 | }
|
---|
631 | gsvFrame_.NumberOfSatellites = nmea0183_.Gsv.NumberOfSatellites;
|
---|
632 | gsvFrame_.Totalmessages = nmea0183_.Gsv.Totalmessages;
|
---|
633 |
|
---|
634 | for ( indexGSV=4*(nmea0183_.Gsv.message_number-1); indexGSV<=(4*nmea0183_.Gsv.message_number)-1; indexGSV++ )
|
---|
635 | {
|
---|
636 | gsvFrame_.SatellitesInView[ indexGSV ][ 0 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SatelliteNumber;
|
---|
637 | gsvFrame_.SatellitesInView[ indexGSV ][ 1 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].ElevationDegrees;
|
---|
638 | gsvFrame_.SatellitesInView[ indexGSV ][ 2 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].AzimuthDegreesTrue;
|
---|
639 | gsvFrame_.SatellitesInView[ indexGSV ][ 3 ] = nmea0183_.Gsv.SatellitesInView[ indexGSV ].SignalToNoiseRatio;
|
---|
640 | }
|
---|
641 |
|
---|
642 | break;
|
---|
643 |
|
---|
644 | case TRAME_HDT:
|
---|
645 | if (!nmea0183_.Hdt.Parse( sentence ))
|
---|
646 | LOG_WARN("Failed to parse the frame " << nmea0183_.Hdt.ErrorMessage.toLatin1().data());
|
---|
647 | hdtFrame_.DegreesTrue = nmea0183_.Hdt.DegreesTrue;
|
---|
648 |
|
---|
649 | break;
|
---|
650 |
|
---|
651 | case TRAME_RMC:
|
---|
652 | if (!nmea0183_.Rmc.Parse( sentence ))
|
---|
653 | LOG_WARN("Failed to parse the frame " << nmea0183_.Rmc.ErrorMessage.toLatin1().data());
|
---|
654 | rmcFrame_.H = nmea0183_.Rmc.Time.time().hour();
|
---|
655 | rmcFrame_.Mi = nmea0183_.Rmc.Time.time().minute();
|
---|
656 | rmcFrame_.S = nmea0183_.Rmc.Time.time().second();
|
---|
657 | rmcFrame_.Ms = nmea0183_.Rmc.Time.time().msec();
|
---|
658 | rmcFrame_.AA = nmea0183_.Rmc.Time.date().year();
|
---|
659 | rmcFrame_.MM = nmea0183_.Rmc.Time.date().month();
|
---|
660 | rmcFrame_.JJ = nmea0183_.Rmc.Time.date().day();
|
---|
661 | rmcFrame_.lat = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Latitude.GetDecimalDegrees());
|
---|
662 | rmcFrame_.dir_lat = ( (nmea0183_.Rmc.Position.Latitude.Northing == North) ? 'N' : 'S');
|
---|
663 | rmcFrame_.lon = Geodesie::Deg2Rad(nmea0183_.Rmc.Position.Longitude.GetDecimalDegrees());
|
---|
664 | rmcFrame_.dir_lon = ( (nmea0183_.Rmc.Position.Longitude.Easting == East) ? 'E' : 'W' );
|
---|
665 | rmcFrame_.magnet_var = nmea0183_.Rmc.MagneticVariation;
|
---|
666 | rmcFrame_.dir_magnet_var = ( (nmea0183_.Rmc.MagneticVariationDirection == East) ? 'E' : 'W');
|
---|
667 | rmcFrame_.mode = -1;
|
---|
668 | if (nmea0183_.Rmc.ModeIndication == "A")
|
---|
669 | rmcFrame_.mode = 1;
|
---|
670 | if (nmea0183_.Rmc.ModeIndication == "D")
|
---|
671 | rmcFrame_.mode = 2;
|
---|
672 | if (nmea0183_.Rmc.ModeIndication == "N")
|
---|
673 | rmcFrame_.mode = 0;
|
---|
674 | rmcFrame_.track_true_north = nmea0183_.Rmc.TrackMadeGoodDegreesTrue;
|
---|
675 | rmcFrame_.valid_data = ( (nmea0183_.Rmc.IsDataValid == True) ? 1 : 0 );
|
---|
676 | rmcFrame_.vitesse = nmea0183_.Rmc.SpeedOverGroundKnots * 1852.0 / 3600.0; // 1 knot = 1852 m/h
|
---|
677 |
|
---|
678 | break;
|
---|
679 |
|
---|
680 | case TRAME_ROT:
|
---|
681 | if (!nmea0183_.Rot.Parse( sentence ))
|
---|
682 | LOG_WARN("Failed to parse the frame " << nmea0183_.Rot.ErrorMessage.toLatin1().data());
|
---|
683 | rotFrame_.RateOfTurn = nmea0183_.Rot.RateOfTurn;
|
---|
684 | rotFrame_.valid_data = ( (nmea0183_.Rot.IsDataValid == True) ? 1 : 0 );
|
---|
685 |
|
---|
686 | break;
|
---|
687 |
|
---|
688 | case TRAME_VTG:
|
---|
689 | if (!nmea0183_.Vtg.Parse( sentence ))
|
---|
690 | LOG_WARN("Failed to parse the frame " << nmea0183_.Vtg.ErrorMessage.toLatin1().data());
|
---|
691 | vtgFrame_.v = nmea0183_.Vtg.SpeedKilometersPerHour;
|
---|
692 | vtgFrame_.track_true_north = nmea0183_.Vtg.TrackDegreesTrue;
|
---|
693 | vtgFrame_.track_magnet_north = nmea0183_.Vtg.TrackDegreesMagnetic;
|
---|
694 |
|
---|
695 | break;
|
---|
696 |
|
---|
697 | case TRAME_ZDA:
|
---|
698 | if (!nmea0183_.Zda.Parse( sentence ))
|
---|
699 | LOG_WARN("Failed to parse the frame " << nmea0183_.Zda.ErrorMessage.toLatin1().data());
|
---|
700 | zdaFrame_.H = nmea0183_.Zda.Time.time().hour();
|
---|
701 | zdaFrame_.Mi = nmea0183_.Zda.Time.time().minute();
|
---|
702 | zdaFrame_.S = nmea0183_.Zda.Time.time().second();
|
---|
703 | zdaFrame_.Ms = nmea0183_.Zda.Time.time().msec();
|
---|
704 | zdaFrame_.AA = nmea0183_.Zda.Time.date().year();
|
---|
705 | zdaFrame_.MM = nmea0183_.Zda.Time.date().month();
|
---|
706 | zdaFrame_.JJ = nmea0183_.Zda.Time.date().day();
|
---|
707 | zdaFrame_.H_offset = nmea0183_.Zda.LocalHourDeviation;
|
---|
708 | zdaFrame_.Mi_offset = nmea0183_.Zda.LocalMinutesDeviation;
|
---|
709 |
|
---|
710 | break;
|
---|
711 |
|
---|
712 | case TRAME_BESTGPSPOSA:
|
---|
713 | stringList = sentence.Sentence.split(";");
|
---|
714 | if (stringList.size()==2) {
|
---|
715 | header = stringList.at(0);
|
---|
716 | data = stringList.at(1);
|
---|
717 |
|
---|
718 | if (mVerbose) {
|
---|
719 | LOG_DEBUG("Header " << stringList.at(0));
|
---|
720 | LOG_DEBUG("Data " << stringList.at(1));
|
---|
721 | }
|
---|
722 |
|
---|
723 | if (parseCPTbestgpsposa(data)) {
|
---|
724 | if (mVerbose) {
|
---|
725 | LOG_DEBUG("parsed TRAME_BESTGPSPOSA");
|
---|
726 | }
|
---|
727 | }
|
---|
728 | }
|
---|
729 | break;
|
---|
730 |
|
---|
731 | case TRAME_RAWIMUSA:
|
---|
732 | stringList = sentence.Sentence.split(";");
|
---|
733 | LOG_DEBUG(sentence.Sentence);
|
---|
734 | if (stringList.size()==2) {
|
---|
735 | header = stringList.at(0);
|
---|
736 | data = stringList.at(1);
|
---|
737 |
|
---|
738 | LOG_DEBUG("Header " << stringList.at(0));
|
---|
739 | LOG_DEBUG("Data " << stringList.at(1));
|
---|
740 |
|
---|
741 | if (parseCPTrawimusa(data))
|
---|
742 | LOG_DEBUG("parsed TRAME_RAWIMUSA");
|
---|
743 | }
|
---|
744 | break;
|
---|
745 | case TRAME_INSPVAA:
|
---|
746 | stringList = sentence.Sentence.split(";");
|
---|
747 | LOG_DEBUG(sentence.Sentence);
|
---|
748 | if (stringList.size()==2) {
|
---|
749 | header = stringList.at(0);
|
---|
750 | data = stringList.at(1);
|
---|
751 |
|
---|
752 | LOG_DEBUG("Header " << stringList.at(0));
|
---|
753 | LOG_DEBUG("Data " << stringList.at(1));
|
---|
754 |
|
---|
755 | if (parseCPTinspvaa(data))
|
---|
756 | LOG_DEBUG("parsed TRAME_INSPVAA");
|
---|
757 |
|
---|
758 |
|
---|
759 | }
|
---|
760 | break;
|
---|
761 |
|
---|
762 | case TRAME_INSCOV:
|
---|
763 | stringList = sentence.Sentence.split(";");
|
---|
764 | LOG_DEBUG(sentence.Sentence);
|
---|
765 | if (stringList.size()==2) {
|
---|
766 | header = stringList.at(0);
|
---|
767 | data = stringList.at(1);
|
---|
768 |
|
---|
769 | LOG_DEBUG("Header " << stringList.at(0));
|
---|
770 | LOG_DEBUG("Data " << stringList.at(1));
|
---|
771 |
|
---|
772 | if (parseCPTinscov(data))
|
---|
773 | LOG_DEBUG("parsed TRAME_INSCOV");
|
---|
774 |
|
---|
775 | }
|
---|
776 | break;
|
---|
777 |
|
---|
778 | default:
|
---|
779 | LOG_ERROR("unknown frame type=" << type);
|
---|
780 | return 0;
|
---|
781 | }
|
---|
782 |
|
---|
783 | return 1;
|
---|
784 | }
|
---|
785 |
|
---|
786 | int 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 |
|
---|
915 | int 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 |
|
---|
939 | int 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 |
|
---|
991 | int 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 |
|
---|
1043 | void 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 | }
|
---|