[89] | 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 | }
|
---|