00001 #pragma ident "$Id: NovatelData.cpp 2741 2011-06-22 16:37:02Z nwu $"
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00045 #include <cstring>
00046 #include "BinUtils.hpp"
00047 #include "StringUtils.hpp"
00048 #include "DayTime.hpp"
00049 #include "EngEphemeris.hpp"
00050 #include "RinexObsHeader.hpp"
00051 #include "icd_200_constants.hpp"
00052 #include "NovatelData.hpp"
00053
00054 using namespace std;
00055 using namespace gpstk::BinUtils;
00056 using namespace gpstk::StringUtils;
00057
00058 static bool debug=false;
00059
00060 namespace gpstk
00061 {
00062
00063
00064 const double CFF=C_GPS_M/OSC_FREQ;
00065 const double wl1=CFF/L1_MULT;
00066 const double wl2=CFF/L2_MULT;
00067 const double PhaseRollover=8388608;
00068
00069
00070 const string NovatelData::RecNames[] = {
00071 string("Unknown"),
00072 string("RGEB obs"),
00073 string("RGEC obs"),
00074 string("POSB pos"),
00075 string("REPB nav"),
00076 string("RCSB sts"),
00077 string("RANGE obs"),
00078 string("RANGECMP obs"),
00079 string("RAWEPHEM nav")
00080 };
00081
00082
00083 bool NovatelData::isNav(void) const
00084 {
00085 switch(rectype) {
00086 case POSB:
00087 case RCSB:
00088 return false;
00089 case REPB:
00090 case RAWEPHEM:
00091 return true;
00092 case RGEB:
00093 case RGEC:
00094 case RANGE:
00095 case RANGECMP:
00096 return false;
00097 case Unknown:
00098 default:
00099 return false;
00100 }
00101 }
00102
00103
00104 bool NovatelData::isObs(void) const
00105 {
00106 switch(rectype) {
00107 case POSB:
00108 case RCSB:
00109 return false;
00110 case REPB:
00111 case RAWEPHEM:
00112 return false;
00113 case RGEB:
00114 case RGEC:
00115 case RANGE:
00116 case RANGECMP:
00117 return true;
00118 case Unknown:
00119 default:
00120 return false;
00121 }
00122 }
00123
00124
00125 bool NovatelData::isAux(void) const
00126 {
00127 switch(rectype) {
00128 case POSB:
00129 case RCSB:
00130 return true;
00131 case REPB:
00132 case RAWEPHEM:
00133 return false;
00134 case RGEB:
00135 case RGEC:
00136 case RANGE:
00137 case RANGECMP:
00138 return false;
00139 case Unknown:
00140 default:
00141 return false;
00142 }
00143 }
00144
00145
00146
00147 bool NovatelData::isOEM2(void) const
00148 {
00149 switch(rectype) {
00150 case POSB:
00151 case RCSB:
00152 case REPB:
00153 case RGEB:
00154 case RGEC:
00155 return true;
00156 case RAWEPHEM:
00157 case RANGE:
00158 case RANGECMP:
00159 case Unknown:
00160 default:
00161 return false;
00162 }
00163 }
00164
00165
00166
00167 bool NovatelData::isOEM4(void) const
00168 {
00169 switch(rectype) {
00170 case RAWEPHEM:
00171 case RANGE:
00172 case RANGECMP:
00173 return true;
00174 case POSB:
00175 case RCSB:
00176 case REPB:
00177 case RGEB:
00178 case RGEC:
00179 case Unknown:
00180 default:
00181 return false;
00182 }
00183 }
00184
00185
00186 bool NovatelData::isValid(void) const
00187 {
00188 switch(rectype) {
00189 case POSB:
00190 case RCSB:
00191 case REPB:
00192 case RAWEPHEM:
00193 case RGEB:
00194 case RGEC:
00195 case RANGE:
00196 case RANGECMP:
00197 if(datasize == 0 || headersize==0) return false;
00198 return true;
00199 case Unknown:
00200 default:
00201 return false;
00202 }
00203 }
00204
00205
00206 void NovatelData::dump(ostream& str) const
00207 {
00208 str << "Record type is " << rectype << endl;
00209 }
00210
00211
00212 void NovatelData::reallyPutRecord(FFStream& s) const
00213 throw(exception, StringUtils::StringException, FFStreamError)
00214 {
00215 FFStreamError e("Novatel::reallyPutRecord() is not implemented");
00216 GPSTK_THROW(e);
00217 }
00218
00219
00220
00221 void NovatelData::reallyGetRecord(FFStream& ffs)
00222 throw(exception, StringUtils::StringException, FFStreamError)
00223 {
00224 try {
00225 if(dynamic_cast<NovatelStream*>(&ffs)) {
00226 NovatelStream& strm = dynamic_cast<NovatelStream&>(ffs);
00227
00228 unsigned char *p0 = &buffer[0];
00229 unsigned char *p1 = &buffer[1];
00230 unsigned char *p2 = &buffer[2];
00231 unsigned char *p3 = &buffer[3];
00232 unsigned char *p4 = &buffer[4];
00233 int i,j,k,failure;
00234 long filepos;
00235
00236 if(debug) cout << "Entered NovatelData::reallyGetRecord()" << endl;
00237
00238 do {
00239
00240
00241 *p0 = *p1;
00242 *p1 = *p2;
00243
00244
00245 try {
00246 strm.read((char *)p2, 1);
00247 }
00248 catch(exception& e) {
00249 if(debug) cout << "read 1 threw std exception: " << e.what() << endl;
00250
00251
00252 }
00253
00254 if(strm.bad()) {
00255 FFStreamError fe("Read error");
00256 GPSTK_THROW(fe);
00257 }
00258 if(strm.eof()) {
00259 if(debug) cout << "Reached EOF" << endl;
00260 break;
00261 }
00262
00263 if(debug) cout << "got char 0x" << hex << uppercase << int(buffer[2])
00264 << dec << endl;
00265
00266
00267 if(*p0==0xAA && *p1==0x44 && *p2==0x11) {
00268
00269 if(debug) cout << "Found OEM2 sync" << endl;
00270
00271
00272 filepos = strm.tellg();
00273 if(debug) cout << "File position " << filepos << endl;
00274
00275
00276 strm.read((char *)p3,9);
00277 if(strm.bad()) {
00278 FFStreamError fe("Read error");
00279 GPSTK_THROW(fe);
00280 }
00281 if(strm.eof()) {
00282 if(debug) cout << "Reached EOF" << endl;
00283 break;
00284 }
00285
00286
00287 if(*p4==0x20) rectype = RGEB;
00288 else if(*p4==0x21) rectype = RGEC;
00289 else if(*p4==0x01) rectype = POSB;
00290 else if(*p4==0x0E) rectype = REPB;
00291 else if(*p4==0x0D) rectype = RCSB;
00292 else rectype = Unknown;
00293 recnum = int(*p4);
00294 intelToHost(recnum);
00295
00296
00297 failure = 0;
00298 if(rectype != Unknown) {
00299
00300
00301 std::memmove(&datasize, &(buffer[8]), 4);
00302 intelToHost(datasize);
00303 if (debug)
00304 cout << "datasize:" << datasize << endl;
00305
00306
00307 if(datasize-12 >= 1024) {
00308
00309
00310 failure = 1;
00311 }
00312 else {
00313 strm.read((char *)&buffer[12],datasize-12);
00314 if(strm.bad()) {
00315 FFStreamError fe("Read error");
00316 GPSTK_THROW(fe);
00317 }
00318 if(strm.eof()) {
00319 if(debug) cout << "Reached EOF" << endl;
00320 break;
00321 }
00322 headersize = 3;
00323
00324
00325
00326 unsigned char checksum = 0;
00327 checksum ^= buffer[0];
00328 checksum ^= buffer[1];
00329 checksum ^= buffer[2];
00330 for(i=4; i<datasize; i++) checksum ^= buffer[i];
00331
00332 if(checksum == buffer[3]) break;
00333 failure = 2;
00334
00335 }
00336 }
00337
00338 if (debug)
00339 cout << "failure=" << failure
00340 << ", rectype=" << rectype
00341 << ", datasize=" << datasize << endl;
00342
00343
00344 if(debug) {
00345 cout << "Failure - ";
00346 if(failure == 0) cout << "type unknown";
00347 else if(failure == 1) cout << "buffer overflow";
00348 else if(failure == 2) cout << "failed checksum";
00349 cout << " for recnum " << recnum
00350 << " with headersize " << headersize
00351 << " and message size " << datasize << endl;
00352 }
00353
00354 strm.seekg(filepos);
00355 datasize = headersize = 0;
00356
00357 }
00358
00359 else if(*p0==0xAA && *p1==0x44 && *p2==0x12) {
00360
00361
00362
00363 if(debug) cout << "Found OEM4 sync" << endl;
00364
00365
00366 filepos = strm.tellg();
00367 if(debug) cout << "File position " << filepos << endl;
00368
00369
00370
00371 strm.read((char *)p3,25);
00372 if(strm.bad()) {
00373 FFStreamError fe("Read error");
00374 GPSTK_THROW(fe);
00375 }
00376 if(strm.eof()) {
00377 if(debug) cout << "Reached EOF" << endl;
00378 break;
00379 }
00380
00381
00382
00383
00384
00385 unsigned char headerLength;
00386 std::memmove(&headerLength, &(buffer[3]), 1); intelToHost(headerLength);
00387 short messageID;
00388 std::memmove(&messageID, &(buffer[4]), 2); intelToHost(messageID);
00389
00390
00391
00392
00393 short messageLength;
00394 std::memmove(&messageLength, &(buffer[8]), 2); intelToHost(messageLength);
00395
00396
00397
00398
00399
00400
00401
00402
00403
00404
00405
00406
00407
00408
00409
00410
00411
00412 datasize = messageLength;
00413 headersize = int(headerLength);
00414 recnum = messageID;
00415
00416 if(headersize != 28) {
00417 Exception e("Header size : expected 28 but found "
00418 + StringUtils::asString(headersize) + " for record ID "
00419 + StringUtils::asString(recnum));
00420 GPSTK_THROW(e);
00421 }
00422
00423 if(debug) cout << "hL " << int(headerLength)
00424 << " ID " << messageID
00425 << " mL " << messageLength
00426
00427
00428
00429
00430 << endl;
00431
00432 if( recnum == 43) rectype = RANGE;
00433 else if(recnum == 140) rectype = RANGECMP;
00434 else if(recnum == 41) rectype = RAWEPHEM;
00435 else rectype = Unknown;
00436
00437 failure=0;
00438 if(rectype != Unknown) {
00439
00440
00441
00442
00443 if(datasize > 65536 || datasize < 0) {
00444
00445
00446 failure = 1;
00447 }
00448 else {
00449 strm.read((char *)&(buffer[28]),datasize);
00450 if(strm.bad()) {
00451 FFStreamError fe("Read error");
00452 GPSTK_THROW(fe);
00453 }
00454 if(strm.eof()) {
00455 datasize = 0;
00456 if(debug) cout << "Reached EOF" << endl;
00457 break;
00458 }
00459 if(debug) cout << "Successfully read message" << endl;
00460
00461
00462
00463
00464
00465
00466 unsigned int checksum =
00467 intelToHost(strm.getData<unsigned int>());
00468
00469
00470 unsigned int check=0,ultemp1,ultemp2;
00471 for(i=0; i<datasize+28; i++) {
00472 ultemp1 = (check >> 8) & 0x00FFFFFFL;
00473 j = ((int)check ^ buffer[i]) & 0xFF;
00474 ultemp2 = j;
00475 for(k=8; k>0; k--) {
00476 if(ultemp2 & 1)
00477 ultemp2 = (ultemp2 >> 1) ^ 0xEDB88320L;
00478 else
00479 ultemp2 >>= 1;
00480 }
00481 check = ultemp1 ^ ultemp2;
00482 }
00483
00484 if(check == checksum) {
00485 if(debug) cout << "checksum ok" << endl;
00486 break;
00487 }
00488 else failure = 2;
00489
00490 }
00491 }
00492
00493
00494 cout << "Failure - ";
00495 if(failure == 0) cout << "type unknown";
00496 else if(failure == 1) cout << "buffer overflow";
00497 else if(failure == 2) cout << "failed checksum";
00498 cout << " for recnum " << recnum
00499 << " with headersize " << headersize
00500 << " and message size " << datasize << endl;
00501
00502 strm.seekg(filepos);
00503 datasize = headersize = 0;
00504
00505 }
00506
00507 else {
00508
00509 if(debug && !(*p1==0xAA && *p2==0x44) && !(*p2==0xAA) )
00510 cout << "Skip a byte " << hex << uppercase << setfill('0')
00511 << setw(2) << int(*p0) << setfill(' ') << endl;
00512 }
00513
00514 } while(1);
00515 }
00516 else {
00517 FFStreamError e("NovatelData tried to read from a non-Novatel file");
00518 GPSTK_THROW(e);
00519 }
00520
00521 if(!isValid()) {
00522 FFStreamError e("Read an invalid Novatel record");
00523 GPSTK_THROW(e);
00524 }
00525
00526 }
00527 catch(Exception e) {
00528 if(debug) cout << "reallyGetRecord caught GPSTK exception " << e << endl;
00529 }
00530 catch(exception e) {
00531 if(debug) cout << "reallyGetRecord caught std exception " << e.what() << endl;
00532 }
00533 catch(...) {
00534 if(debug) cout << "reallyGetRecord caught an unknown exception" << endl;
00535 }
00536
00537 }
00538
00539
00540
00541 NovatelData::operator RinexNavData()
00542 throw(Exception)
00543 {
00544 if(!isValid() || !isNav()) {
00545 Exception e("Invalid or non-Nav record");
00546 GPSTK_THROW(e);
00547 }
00548
00549 int i,j,k;
00550 long templ;
00551 EngEphemeris eeph;
00552
00553 if(rectype == RAWEPHEM) {
00554
00555
00556
00557 unsigned char headerLength;
00558 std::memmove(&headerLength, &(buffer[3]), 1); intelToHost(headerLength);
00559 short messageID;
00560 std::memmove(&messageID, &(buffer[4]), 2); intelToHost(messageID);
00561 char messageType;
00562 std::memmove(&messageType, &(buffer[6]), 1); intelToHost(messageType);
00563 char portAddress;
00564 std::memmove(&portAddress, &(buffer[7]), 1); intelToHost(portAddress);
00565 short messageLength;
00566 std::memmove(&messageLength, &(buffer[8]), 2); intelToHost(messageLength);
00567 short sequence;
00568 std::memmove(&sequence, &(buffer[10]), 2); intelToHost(sequence);
00569 char idleTime;
00570 std::memmove(&idleTime, &(buffer[12]), 1); intelToHost(idleTime);
00571 char timeStatus;
00572 std::memmove(&timeStatus, &(buffer[13]), 1); intelToHost(timeStatus);
00573 short week;
00574 std::memmove(&week, &(buffer[14]), 2); intelToHost(week);
00575 long msecOfWeek;
00576 std::memmove(&msecOfWeek, &(buffer[16]), 4); intelToHost(msecOfWeek);
00577 long rxStatus;
00578 std::memmove(&rxStatus, &(buffer[20]), 4); intelToHost(rxStatus);
00579 short reserved;
00580 std::memmove(&reserved, &(buffer[24]), 2); intelToHost(reserved);
00581 short rxSWVersion;
00582 std::memmove(&rxSWVersion, &(buffer[26]), 2); intelToHost(rxSWVersion);
00583
00584
00585
00586 short prn,track=1;
00587 long gpsSOW;
00588
00589
00590 std::memmove(&templ, &(buffer[28]), 4);
00591 intelToHost(templ);
00592 prn = short(templ);
00593 std::memmove(&gpsWeek, &(buffer[32]), 4);
00594 intelToHost(gpsWeek);
00595 std::memmove(&gpsSOW, &(buffer[36]), 4);
00596 intelToHost(gpsSOW);
00597
00598
00599 long subframe[10];
00600 for(j=0; j<3; j++) {
00601 k = 40 + j*30;
00602
00603 if(debug) {
00604 cout << "Subframe " << setfill('0') << j+1;
00605 for(i=0; i<30; i++)
00606 cout << " " << hex << uppercase << setw(2) << int(buffer[k+i]);
00607 cout << dec << setfill(' ') << endl;
00608 }
00609
00610 for(i=0; i<10; i++) {
00611 subframe[i] = (buffer[k] << 22)+(buffer[k+1] << 14)+(buffer[k+2] << 6);
00612 k += 3;
00613 }
00614 if(!eeph.addSubframe(subframe,gpsWeek,prn,track)){
00615 if(debug) cout << "Failed to convert RAWEPH subframe " << j+1
00616 << ", prn " << prn << " at time " << gpsWeek << " " << gpsSOW
00617 << endl;
00618 }
00619 }
00620 }
00621
00622 else if(rectype == REPB) {
00623
00624 long prn;
00625 short track=1;
00626
00627
00628 std::memmove(&prn,&(buffer[12]), 4);
00629 intelToHost(prn);
00630
00631
00632 if(gpsWeek == -1) {
00633 DayTime sysTime;
00634 gpsWeek = long(sysTime.GPSfullweek());
00635 }
00636
00637
00638 long subframe[10];
00639 for(j=0; j<3; j++) {
00640 k = 16 + j*30;
00641
00642 if(debug) {
00643 cout << "Subframe " << setfill('0') << j+1;
00644 for(i=0; i<30; i++)
00645 cout << " " << hex << uppercase << setw(2) << int(buffer[k+i]);
00646 cout << dec << setfill(' ') << endl;
00647 }
00648
00649 for(i=0; i<10; i++) {
00650 subframe[i] = (buffer[k] << 22)+(buffer[k+1] << 14)+(buffer[k+2] << 6);
00651 k += 3;
00652 }
00653 if(!eeph.addSubframe(subframe,gpsWeek,short(prn),track)){
00654 if(debug) cout << "Failed to convert REPB subframe " << j+1
00655 << ", prn " << prn << endl;
00656 }
00657 }
00658
00659 }
00660
00661
00662 RinexNavData rnd(eeph);
00663
00664 return rnd;
00665
00666 }
00667
00668
00669
00670 NovatelData::operator RinexObsData()
00671 throw(Exception)
00672 {
00673 if(!isValid() || !isObs()) {
00674 Exception e("Invalid or non-Obs record");
00675 GPSTK_THROW(e);
00676 }
00677
00678 int i,j;
00679 int16_t temps;
00680 int32_t nobs;
00681 SatID sat;
00682 RinexObsData rod;
00683 RinexObsData::RinexDatum rd;
00684 RinexObsData::RinexSatMap::iterator satit;
00685 RinexObsData::RinexObsTypeMap::iterator obsit;
00686
00687 if( rectype == RGEB) {
00688
00689
00690 }
00691
00692 else if(rectype == RGEC) {
00693
00694
00695 if(debug) {
00696 cout << "Header " << setfill('0') << hex << uppercase;
00697 for(i=0; i<24; i++) cout << " " << setw(2) << int(buffer[i]);
00698 cout << dec << setfill(' ') << endl;
00699 }
00700
00701
00702 std::memmove(&temps, &(buffer[12]), 2);
00703 intelToHost(temps);
00704 nobs = int32_t(temps);
00705
00706
00707 std::memmove(&temps, &(buffer[14]), 2);
00708 intelToHost(temps);
00709
00710
00711 if(gpsWeek == -1) {
00712 DayTime sysTime;
00713 gpsWeek = long(sysTime.GPSfullweek());
00714 }
00715 gpsWeek = long(temps) + 1024*(gpsWeek/1024);
00716
00717
00718 int32_t gpsSOW;
00719 std::memmove(&gpsSOW, &(buffer[16]), 4);
00720 intelToHost(gpsSOW);
00721
00722
00723 int32_t rxStatus;
00724 std::memmove(&rxStatus, &(buffer[20]), 4);
00725
00726
00727 if (debug)
00728 cout << "gpsWeek:" << gpsWeek << " sow:" << gpsSOW/100.0 << endl;
00729 rod.time = DayTime(gpsWeek,gpsSOW/100.);
00730 rod.epochFlag = 0;
00731 rod.clockOffset = 0.0;
00732 rod.numSvs = 0;
00733
00734
00735 for(i=0; i<nobs; i++) {
00736 uint32_t data[5];
00737 for(j=0; j<5; j++)
00738 std::memmove(&data[j], &(buffer[24+i*20+j*4]), 4);
00739
00740 int prn = int(data[0] & 0x0000003FL);
00741
00742 double SNR = double((data[0] & 0x000007C0L) >> 6);
00743
00744 double locktime = double((data[0] & 0xFFFFF800L) >> 11);
00745
00746 double Ph;
00747 if(data[1] & 0x80000000L)
00748 Ph = double((data[1] ^ 0x7FFFFFFFL) + 1);
00749 else
00750 Ph = double(data[1]);
00751
00752 double Doppler = double((data[2] & 0xFFFFFFF0L) >> 4);
00753 if(data[2] & 0x80000000L)
00754 Doppler = -double((((data[2] & 0xFFFFFFF0L) ^ 0xFFFFFFF0L) >> 4)+1);
00755
00756
00757 double Pr = double(data[2] & 0x0000000FL) * 4294967296.
00758 + double(data[3]);
00759
00760 if(data[2] & 0x00000008L)
00761 Pr = -double((data[2] & 0x0000000FL) ^ 0x0000000FL) * 4294967296.
00762 - double(data[3] ^ 0xFFFFFFFFL + 1);
00763
00764 double SdPh = int(data[4] & 0x0000000FL);
00765
00766 double SdPr = double((data[4] & 0x000000F0L) >> 4);
00767
00768 long TrackStatus = long((data[4] & 0xFFFFFF00L) >> 8);
00769
00770
00771
00772 intelToHost(prn);
00773 intelToHost(SNR);
00774 intelToHost(locktime);
00775 intelToHost(Ph);
00776 intelToHost(Doppler);
00777 intelToHost(Pr);
00778 intelToHost(SdPr);
00779 intelToHost(SdPh);
00780
00781
00782 SNR += 20.;
00783 locktime /= 32.;
00784 Doppler /= 256.;
00785 Pr /= 128.;
00786 Ph /= 256.;
00787 SdPr = (SdPr + 1.)/16.;
00788 SdPh = (SdPh + 1)/512.;
00789
00790
00791
00792 int TrackState = int( TrackStatus & 0x0000000FL);
00793 int Channel = int((TrackStatus & 0x000001F0L) >> 4);
00794 bool PhaseLock = bool(TrackStatus & 0x00000200L);
00795 bool ParityKnown = bool(TrackStatus & 0x00000400L);
00796 bool CodeLock = bool(TrackStatus & 0x00000800L);
00797 int Frequency = int((TrackStatus & 0x00100000L) >> 20);
00798
00799 int CodeType = int((TrackStatus & 0x00600000L) >> 21);
00800
00801 if(!PhaseLock || !CodeLock) continue;
00802
00803
00804
00805 double ADRrolls = ((-Pr/(Frequency==0 ? wl1 : wl2))-Ph)/PhaseRollover;
00806 Ph += long(ADRrolls + (ADRrolls > 0 ? 0.5 : -0.5)) * PhaseRollover;
00807
00808
00809
00810
00811
00812 sat = SatID(prn,SatID::systemGPS);
00813 satit = rod.obs.find(sat);
00814 if(satit == rod.obs.end()) {
00815 RinexObsData::RinexObsTypeMap rotm;
00816 rod.obs[sat] = rotm;
00817 rod.numSvs++;
00818 satit = rod.obs.find(sat);
00819 }
00820
00821
00822 RinexObsData::RinexObsTypeMap& obs = satit->second;
00823 if(Frequency == 0) {
00824 rd.ssi = rd.lli = 0; rd.data = -Ph;
00825 obs[RinexObsHeader::L1] = rd;
00826
00827 rd.ssi = rd.lli = 0; rd.data = Pr;
00828 if(CodeType == 0) obs[RinexObsHeader::C1] = rd;
00829 else obs[RinexObsHeader::P1] = rd;
00830
00831 rd.ssi = rd.lli = 0; rd.data = -Doppler;
00832 obs[RinexObsHeader::D1] = rd;
00833
00834 rd.ssi = rd.lli = 0; rd.data = SNR;
00835 obs[RinexObsHeader::S1] = rd;
00836 }
00837 else {
00838 rd.ssi = rd.lli = 0; rd.data = Ph;
00839 obs[RinexObsHeader::L2] = rd;
00840
00841 rd.ssi = rd.lli = 0; rd.data = Pr;
00842 obs[RinexObsHeader::P2] = rd;
00843
00844 rd.ssi = rd.lli = 0; rd.data = -Doppler;
00845 obs[RinexObsHeader::D2] = rd;
00846
00847 rd.ssi = rd.lli = 0; rd.data = SNR;
00848 obs[RinexObsHeader::S2] = rd;
00849 }
00850
00851 }
00852
00853 }
00854
00855 else {
00856
00857
00858
00859 uint8_t headerLength;
00860 std::memmove(&headerLength, &(buffer[3]), 1); intelToHost(headerLength);
00861 int16_t messageID;
00862 std::memmove(&messageID, &(buffer[4]), 2); intelToHost(messageID);
00863 int8_t messageType;
00864 std::memmove(&messageType, &(buffer[6]), 1); intelToHost(messageType);
00865 int8_t portAddress;
00866 std::memmove(&portAddress, &(buffer[7]), 1); intelToHost(portAddress);
00867 int16_t messageLength;
00868 std::memmove(&messageLength, &(buffer[8]), 2); intelToHost(messageLength);
00869 int16_t sequence;
00870 std::memmove(&sequence, &(buffer[10]), 2); intelToHost(sequence);
00871 int8_t idleTime;
00872 std::memmove(&idleTime, &(buffer[12]), 1); intelToHost(idleTime);
00873 int8_t timeStatus;
00874 std::memmove(&timeStatus, &(buffer[13]), 1); intelToHost(timeStatus);
00875 int16_t week;
00876 std::memmove(&week, &(buffer[14]), 2); intelToHost(week);
00877 int32_t msecOfWeek;
00878 std::memmove(&msecOfWeek, &(buffer[16]), 4); intelToHost(msecOfWeek);
00879 int32_t rxStatus;
00880 std::memmove(&rxStatus, &(buffer[20]), 4); intelToHost(rxStatus);
00881 int16_t reserved;
00882 std::memmove(&reserved, &(buffer[24]), 2); intelToHost(reserved);
00883 int16_t rxSWVersion;
00884 std::memmove(&rxSWVersion, &(buffer[26]), 2); intelToHost(rxSWVersion);
00885
00886
00887 rod.time = DayTime(week,double(msecOfWeek)/1000.);
00888 rod.epochFlag = 0;
00889 rod.clockOffset = 0.0;
00890
00891 if( rectype == RANGE) {
00892
00893
00894 nobs = 0;
00895 std::memmove(&nobs, &(buffer[28]), 4);
00896 intelToHost(nobs);
00897
00898 rod.numSvs = 0;
00899 for(i=0; i<nobs; i++) {
00900 uint16_t prn,reserved;
00901 uint32_t TrackStatus;
00902 float PrStd,PhStd,Doppler,SNR,locktime;
00903 double Pr,Ph;
00904
00905 std::memmove(&prn, &(buffer[32+i*44]), 2);
00906 intelToHost(prn);
00907 std::memmove(&reserved, &(buffer[34+i*44]), 2);
00908 intelToHost(reserved);
00909 std::memmove(&Pr, &(buffer[36+i*44]), 8);
00910 intelToHost(Pr);
00911 std::memmove(&PrStd, &(buffer[44+i*44]), 4);
00912 intelToHost(PrStd);
00913 std::memmove(&Ph, &(buffer[48+i*44]), 8);
00914 intelToHost(Ph);
00915 std::memmove(&PhStd, &(buffer[56+i*44]), 4);
00916 intelToHost(PhStd);
00917 std::memmove(&Doppler, &(buffer[60+i*44]), 4);
00918 intelToHost(Doppler);
00919 std::memmove(&SNR, &(buffer[64+i*44]), 4);
00920 intelToHost(SNR);
00921 std::memmove(&locktime, &(buffer[68+i*44]), 4);
00922 intelToHost(locktime);
00923 std::memmove(&TrackStatus, &(buffer[72+i*44]), 4);
00924 intelToHost(TrackStatus);
00925
00926
00927
00928 int TrackState = int( TrackStatus & 0x0000001FL);
00929 int Channel = int((TrackStatus & 0x000003E0L) >> 5);
00930 bool PhaseLock = bool(TrackStatus & 0x00000400L);
00931 bool CodeLock = bool(TrackStatus & 0x00001000L);
00932 int Frequency = int((TrackStatus & 0x00600000L) >> 21);
00933
00934 int CodeType = int((TrackStatus & 0x03800000L) >> 23);
00935 bool HalfCycle = bool(TrackStatus & 0x10000000L);
00936
00937 if(!PhaseLock || !CodeLock) continue;
00938
00939
00940 sat = SatID(prn,SatID::systemGPS);
00941 satit = rod.obs.find(sat);
00942 if(satit == rod.obs.end()) {
00943 RinexObsData::RinexObsTypeMap rotm;
00944 rod.obs[sat] = rotm;
00945 rod.numSvs++;
00946 satit = rod.obs.find(sat);
00947 }
00948
00949
00950 RinexObsData::RinexObsTypeMap& obs = satit->second;
00951 if(Frequency == 0) {
00952 rd.ssi = rd.lli = 0; rd.data = -Ph;
00953 obs[RinexObsHeader::L1] = rd;
00954
00955 rd.ssi = rd.lli = 0; rd.data = Pr;
00956 if(CodeType == 0) obs[RinexObsHeader::C1] = rd;
00957 else obs[RinexObsHeader::P1] = rd;
00958
00959 rd.ssi = rd.lli = 0; rd.data = Doppler;
00960 obs[RinexObsHeader::D1] = rd;
00961
00962 rd.ssi = rd.lli = 0; rd.data = SNR;
00963 obs[RinexObsHeader::S1] = rd;
00964 }
00965 else {
00966 rd.ssi = rd.lli = 0; rd.data = -Ph;
00967 obs[RinexObsHeader::L2] = rd;
00968
00969 rd.ssi = rd.lli = 0; rd.data = Pr;
00970 obs[RinexObsHeader::P2] = rd;
00971
00972 rd.ssi = rd.lli = 0; rd.data = Doppler;
00973 obs[RinexObsHeader::D2] = rd;
00974
00975 rd.ssi = rd.lli = 0; rd.data = SNR;
00976 obs[RinexObsHeader::S2] = rd;
00977 }
00978
00979 }
00980
00981 }
00982
00983 else if(rectype == RANGECMP) {
00984
00985
00986 nobs = 0;
00987 std::memmove(&nobs, &(buffer[28]), 4);
00988 intelToHost(nobs);
00989
00990 rod.numSvs = 0;
00991 for(i=0; i<nobs; i++) {
00992 uint32_t data[6];
00993 for(j=0; j<6; j++)
00994 std::memmove(&data[j], &(buffer[32+i*24+j*4]), 4);
00995
00996 long TrackStatus = data[0];
00997
00998
00999
01000
01001
01002
01003 double Doppler = double((data[1] & 0x0FFFFFFFL));
01004 if(data[1] & 0x08000000L)
01005 Doppler = -double(((data[1] & 0x0FFFFFFFL) ^ 0x0FFFFFFFL) + 1);
01006 double Pr = double((data[1] & 0xF0000000L) >> 28)
01007 + double(data[2]) * 16.;
01008 double Ph = double(data[3]);
01009 int SdPrCode = int(data[4] & 0x0000000FL);
01010 double SdPh = double((data[4] & 0x000000F0L) >> 4);
01011 int prn = int((data[4] & 0x0000FF00L) >> 8);
01012 double locktime = double((data[4] & 0xFFFF0000L) >> 16)
01013 + double(data[5] & 0x0000001FL);
01014 double SNR = double((data[5] & 0x000003E0L) >> 5);
01015
01016
01017
01018 intelToHost(Doppler);
01019 intelToHost(Pr);
01020 intelToHost(Ph);
01021 intelToHost(SdPrCode);
01022 intelToHost(SdPh);
01023 intelToHost(prn);
01024 intelToHost(locktime);
01025 intelToHost(SNR);
01026
01027
01028 Doppler /= 256.;
01029 Pr /= 128.;
01030 Ph /= 256.;
01031 double SdPr;
01032 switch(SdPrCode) {
01033
01034 case 0: SdPr = 0.050; break;
01035 case 1: SdPr = 0.075; break;
01036 case 2: SdPr = 0.113; break;
01037 case 3: SdPr = 0.169; break;
01038 case 4: SdPr = 0.253; break;
01039 case 5: SdPr = 0.380; break;
01040 case 6: SdPr = 0.570; break;
01041 case 7: SdPr = 0.854; break;
01042 case 8: SdPr = 1.281; break;
01043 case 9: SdPr = 2.375; break;
01044 case 10: SdPr = 4.750; break;
01045 case 11: SdPr = 9.500; break;
01046 case 12: SdPr = 19.000; break;
01047 case 13: SdPr = 38.000; break;
01048 case 14: SdPr = 76.000; break;
01049 case 15: SdPr = 152.000; break;
01050 default: SdPr = 0.00; break;
01051 }
01052 SdPh = (SdPh + 1)/512.;
01053 locktime /= 32.;
01054 SNR += 20.;
01055
01056
01057
01058
01059 int TrackState = int( TrackStatus & 0x0000001FL);
01060 int Channel = int((TrackStatus & 0x000003E0L) >> 5);
01061 bool PhaseLock = bool(TrackStatus & 0x00000400L);
01062 bool CodeLock = bool(TrackStatus & 0x00001000L);
01063 int Frequency = int((TrackStatus & 0x00600000L) >> 21);
01064
01065 int CodeType = int((TrackStatus & 0x03800000L) >> 23);
01066 bool HalfCycle = bool(TrackStatus & 0x10000000L);
01067
01068 if(!PhaseLock || !CodeLock) continue;
01069
01070
01071
01072 double ADRrolls = ((Pr/(Frequency==0 ? wl1 : wl2)) + Ph)/PhaseRollover;
01073 Ph -= long(ADRrolls + (ADRrolls > 0 ? 0.5 : -0.5)) * PhaseRollover;
01074
01075
01076
01077
01078
01079
01080
01081
01082 sat = SatID(prn,SatID::systemGPS);
01083 satit = rod.obs.find(sat);
01084 if(satit == rod.obs.end()) {
01085 RinexObsData::RinexObsTypeMap rotm;
01086 rod.obs[sat] = rotm;
01087 rod.numSvs++;
01088 satit = rod.obs.find(sat);
01089 }
01090
01091
01092 RinexObsData::RinexObsTypeMap& obs = satit->second;
01093 if(Frequency == 0) {
01094 rd.ssi = rd.lli = 0; rd.data = -Ph;
01095 obs[RinexObsHeader::L1] = rd;
01096
01097 rd.ssi = rd.lli = 0; rd.data = Pr;
01098 if(CodeType == 0) obs[RinexObsHeader::C1] = rd;
01099 else obs[RinexObsHeader::P1] = rd;
01100
01101 rd.ssi = rd.lli = 0; rd.data = Doppler;
01102 obs[RinexObsHeader::D1] = rd;
01103
01104 rd.ssi = rd.lli = 0; rd.data = SNR;
01105 obs[RinexObsHeader::S1] = rd;
01106 }
01107 else {
01108 rd.ssi = rd.lli = 0; rd.data = -Ph;
01109 obs[RinexObsHeader::L2] = rd;
01110
01111 rd.ssi = rd.lli = 0; rd.data = Pr;
01112 obs[RinexObsHeader::P2] = rd;
01113
01114 rd.ssi = rd.lli = 0; rd.data = Doppler;
01115 obs[RinexObsHeader::D2] = rd;
01116
01117 rd.ssi = rd.lli = 0; rd.data = SNR;
01118 obs[RinexObsHeader::S2] = rd;
01119 }
01120
01121 }
01122
01123 }
01124
01125 }
01126
01127 return rod;
01128
01129 }
01130
01131 }