NovatelData.cpp

Go to the documentation of this file.
00001 #pragma ident "$Id: NovatelData.cpp 2741 2011-06-22 16:37:02Z nwu $"
00002 
00003 //============================================================================
00004 //
00005 //  This file is part of GPSTk, the GPS Toolkit.
00006 //
00007 //  The GPSTk is free software; you can redistribute it and/or modify
00008 //  it under the terms of the GNU Lesser General Public License as published
00009 //  by the Free Software Foundation; either version 2.1 of the License, or
00010 //  any later version.
00011 //
00012 //  The GPSTk is distributed in the hope that it will be useful,
00013 //  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014 //  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00015 //  GNU Lesser General Public License for more details.
00016 //
00017 //  You should have received a copy of the GNU Lesser General Public
00018 //  License along with GPSTk; if not, write to the Free Software Foundation,
00019 //  Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020 //
00021 //  Copyright 2004, The University of Texas at Austin
00022 //
00023 //============================================================================
00024 
00025 //============================================================================
00026 //
00027 //This software developed by Applied Research Laboratories at the University of
00028 //Texas at Austin, under contract to an agency or agencies within the U.S.
00029 //Department of Defense. The U.S. Government retains all rights to use,
00030 //duplicate, distribute, disclose, or release this software.
00031 //
00032 //Pursuant to DoD Directive 523024
00033 //
00034 // DISTRIBUTION STATEMENT A: This software has been approved for public
00035 //                           release, distribution is unlimited.
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       // True if this record belongs to OEM2 receivers
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       // True if this record belongs to OEM4 receivers
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             // read loop
00238          do {
00239 
00240             // move data down by 1 byte
00241             *p0 = *p1;
00242             *p1 = *p2;
00243 
00244             // get another character
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                //FFStreamError fe(string("std exception: ")+e.what());
00251                //GPSTK_THROW(fe);
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             // look for sync bytes
00267             if(*p0==0xAA && *p1==0x44 && *p2==0x11) {
00268                // -------------------------------------------------- OEM2
00269                if(debug) cout << "Found OEM2 sync" << endl;
00270 
00271                   // save position in case of failure
00272                filepos = strm.tellg();
00273                if(debug) cout << "File position " << filepos << endl;
00274 
00275                   // read 9 more characters into buffer, giving total of 12
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                   // read the record ID
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                   // read the rest of the record
00297                failure = 0;
00298                if(rectype != Unknown) {
00299 
00300                      // get the size of the record
00301                   std::memmove(&datasize, &(buffer[8]), 4);
00302                   intelToHost(datasize);
00303                   if (debug)
00304                      cout << "datasize:" << datasize << endl;
00305 
00306                      // read the rest of the record
00307                   if(datasize-12 >= 1024) {
00308                      //FFStreamError fe("Read error - buffer overflow");
00309                      //GPSTK_THROW(fe);
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;             // just the sync bytes
00323 
00324                         // compute the checksum
00325                         // Ref OEM2 manual
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;    // success
00333                      failure = 2;
00334 
00335                   }  // end if datasize fits into buffer
00336                }  // end if record type != unknown
00337 
00338                if (debug)
00339                   cout << "failure=" << failure
00340                        << ", rectype=" << rectype
00341                        << ", datasize=" << datasize << endl;
00342 
00343                   // failure - either type unknown, buffer overflow or failed checksum
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);          // rewind to just after the sync bytes
00355                datasize = headersize = 0;
00356 
00357             }  // end if OEM2 sync
00358 
00359             else if(*p0==0xAA && *p1==0x44 && *p2==0x12) {
00360                // -------------------------------------------------- OEM4
00361                // Ref OEM4 Manual pg 15
00362 
00363                if(debug) cout << "Found OEM4 sync" << endl;
00364 
00365                   // save position in case of failure
00366                filepos = strm.tellg();
00367                if(debug) cout << "File position " << filepos << endl;
00368 
00369                   // ---------------------------------------
00370                   // read header, 25 characters, into buffer
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                   // parse the header
00382                   // Ref OEM4 Manual pg 16
00383                   // (only need some of the data here - cast to Rinex functions
00384                   // will parse the whole thing)
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                //char messageType;
00390                //memmove(&messageType, &(buffer[6]), 1);   intelToHost(messageType);
00391                //char portAddress;
00392                //memmove(&portAddress, &(buffer[7]), 1);   intelToHost(portAddress);
00393                short messageLength;
00394                std::memmove(&messageLength, &(buffer[8]), 2); intelToHost(messageLength);
00395                //short sequence;
00396                //memmove(&sequence, &(buffer[10]), 2);     intelToHost(sequence);
00397                //char idleTime;
00398                //memmove(&idleTime, &(buffer[12]), 1);     intelToHost(idleTime);
00399                //char timeStatus;
00400                //memmove(&timeStatus, &(buffer[13]), 1);   intelToHost(timeStatus);
00401                //short week;
00402                //memmove(&week, &(buffer[14]), 2);         intelToHost(week);
00403                //long msecOfWeek;
00404                //memmove(&msecOfWeek, &(buffer[16]), 4);   intelToHost(msecOfWeek);
00405                //long rxStatus;
00406                //memmove(&rxStatus, &(buffer[20]), 4);     intelToHost(rxStatus);
00407                //short reserved;
00408                //memmove(&reserved, &(buffer[24]), 2);     intelToHost(reserved);
00409                //short rxSWVersion;
00410                //memmove(&rxSWVersion, &(buffer[26]), 2);  intelToHost(rxSWVersion);
00411 
00412                datasize = messageLength;
00413                headersize = int(headerLength);
00414                recnum = messageID;
00415 
00416                if(headersize != 28) {   // manual warns that changes may be made
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                      //<< " seq " << sequence
00427                      //<< " week " << week
00428                      //<< " msow " << msecOfWeek
00429                      //<< " rxver " << rxSWVersion
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                      // read the data message, but don't overwrite the header
00442                      // first check against buffer overflow
00443                   if(datasize > 65536 || datasize < 0) {
00444                      //FFStreamError fe("Read error - buffer overflow");
00445                      //GPSTK_THROW(fe);
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;         // mark a bad record
00456                         if(debug) cout << "Reached EOF" << endl;
00457                         break;
00458                      }
00459                      if(debug) cout << "Successfully read message" << endl;
00460 
00461                         // ---------------------------------------
00462                         // validate with 32-bit CRC
00463                         // cf. Ref OEM4 manual pg 21.
00464 
00465                         // get the checksum at the end
00466                      unsigned int checksum =
00467                         intelToHost(strm.getData<unsigned int>());
00468 
00469                         // calculate the checksum of the header(even sync)+data
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                   }  // end if datasize+28 < buffersize
00491                }  // end if rectype != Unknown
00492 
00493                   // failure - either type unknown, buffer overflow or failed checksum
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;               // marks an invalid object
00504 
00505             }  // end if OEM4 sync
00506 
00507             else {                                       // skip these bytes
00508                   // print only if sync is not underway
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);   // end read loop
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    }  // end NovatelData::reallyGetRecord
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) {                    // OEM4
00554 
00555             // parse header
00556             // Ref OEM4 Manual pg 16
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             // parse data
00585             // Ref OEM4 Manual pg 206
00586          short prn,track=1;
00587          long gpsSOW;
00588 
00589             // get PRN and timetag
00590          std::memmove(&templ, &(buffer[28]), 4);
00591          intelToHost(templ);
00592          prn = short(templ);
00593          std::memmove(&gpsWeek, &(buffer[32]), 4);      // long gpsWeek is member data
00594          intelToHost(gpsWeek);
00595          std::memmove(&gpsSOW, &(buffer[36]), 4);
00596          intelToHost(gpsSOW);
00597 
00598             // convert the 3 subframes and create EngEphemeris
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       }  // end RAWEPH record
00621 
00622       else if(rectype == REPB) {                   // OEM2
00623 
00624          long prn;
00625          short track=1;
00626 
00627          // get PRN
00628          std::memmove(&prn,&(buffer[12]), 4);
00629          intelToHost(prn);
00630 
00631          // be sure week is defined
00632          if(gpsWeek == -1) {
00633             DayTime sysTime;
00634             gpsWeek = long(sysTime.GPSfullweek());
00635          }
00636 
00637             // convert the 3 subframes and create EngEphemeris
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       }  // end REPB record
00660 
00661       // convert it to Rinex
00662       RinexNavData rnd(eeph);
00663 
00664       return rnd;
00665 
00666    }  // end NovatelData::operator RinexNavData()
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;        // number of observation records (may be 2/PRN: L1 and L2)
00681       SatID sat;
00682       RinexObsData rod;     // this will be returned
00683       RinexObsData::RinexDatum rd;
00684       RinexObsData::RinexSatMap::iterator satit;
00685       RinexObsData::RinexObsTypeMap::iterator obsit;
00686 
00687       if(     rectype == RGEB) {             // OEM2
00688 
00689 
00690       }  // end RGEB record
00691 
00692       else if(rectype == RGEC) {             // OEM2
00693             // Ref OEM2 Manual pg 97
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             // number of observation records to follow
00702          std::memmove(&temps, &(buffer[12]), 2);
00703          intelToHost(temps);
00704          nobs = int32_t(temps);
00705 
00706             // GPS week (long gpsWeek is member data)
00707          std::memmove(&temps, &(buffer[14]), 2);
00708          intelToHost(temps);
00709 
00710             // resolve the week number ambiguity
00711          if(gpsWeek == -1) {
00712             DayTime sysTime;
00713             gpsWeek = long(sysTime.GPSfullweek());
00714          }
00715          gpsWeek = long(temps) + 1024*(gpsWeek/1024);
00716 
00717             // seconds of week * 100
00718          int32_t gpsSOW;
00719          std::memmove(&gpsSOW, &(buffer[16]), 4);
00720          intelToHost(gpsSOW);
00721 
00722             // receiver status
00723          int32_t rxStatus;
00724          std::memmove(&rxStatus, &(buffer[20]), 4);
00725 
00726             // put timetag into rod
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;     // don't have it ?
00732          rod.numSvs = 0;
00733 
00734             // loop over observation records
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)     // 2s complement
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)     // 2s complement
00754                Doppler = -double((((data[2] & 0xFFFFFFF0L) ^ 0xFFFFFFF0L) >> 4)+1);
00755 
00756             //                                               this is 0c++/4.3.2/tr1/exp_integral.tcc:xFFFFFFFF + 1
00757             double Pr       =  double(data[2] & 0x0000000FL) * 4294967296.
00758                              + double(data[3]);
00759             // could the pseudorange ever be negative?
00760             if(data[2] & 0x00000008L)     // 2s complement
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             // the rest are reserved
00770 
00771             // swap bytes
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             // convert to physical units
00782             SNR += 20.;             // dB-Hz, but 51 means >=51, and 20 means <=20.
00783             locktime /= 32.;        // sec
00784             Doppler /= 256.;        // Hz
00785             Pr /= 128.;             // m
00786             Ph /= 256.;             // cycles
00787             SdPr = (SdPr + 1.)/16.; // m
00788             SdPh = (SdPh + 1)/512.; // cycles
00789 
00790             // break out the TrackStatus
00791             // cf. Table 5-6, pg 95 of OEM2 manual
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); // 0:L1 1:L2
00798             // CodeType is 0: CA 1: P 2: Pcodeless
00799             int CodeType   = int((TrackStatus & 0x00600000L) >> 21);
00800 
00801             if(!PhaseLock || !CodeLock) continue;
00802 
00803             // correct the phase for rollovers
00804             // ref. OEM2 manual pg 97
00805             double ADRrolls = ((-Pr/(Frequency==0 ? wl1 : wl2))-Ph)/PhaseRollover;
00806             Ph += long(ADRrolls + (ADRrolls > 0 ? 0.5 : -0.5)) * PhaseRollover;
00807 
00808             //apparently the Novatel convert utility ignores this too
00809             //ignore if(!ParityKnown) Ph = 0.0;
00810 
00811             // fill RinexObsData rod
00812             sat = SatID(prn,SatID::systemGPS);
00813             satit = rod.obs.find(sat);          // find the sat
00814             if(satit == rod.obs.end()) {        // not there - add this sat
00815                RinexObsData::RinexObsTypeMap rotm;
00816                rod.obs[sat] = rotm;
00817                rod.numSvs++;
00818                satit = rod.obs.find(sat);       // now find it
00819             }
00820 
00821             // for convenience, reference the obs data map
00822             RinexObsData::RinexObsTypeMap& obs = satit->second;
00823             if(Frequency == 0) {       // frequency = L1
00824                rd.ssi = rd.lli = 0; rd.data = -Ph;
00825                obs[RinexObsHeader::L1] = rd;                         // L1
00826 
00827                rd.ssi = rd.lli = 0; rd.data = Pr;
00828                if(CodeType == 0) obs[RinexObsHeader::C1] = rd;       // C1
00829                else              obs[RinexObsHeader::P1] = rd;       // P1
00830 
00831                rd.ssi = rd.lli = 0; rd.data = -Doppler;
00832                obs[RinexObsHeader::D1] = rd;                         // D1
00833 
00834                rd.ssi = rd.lli = 0; rd.data = SNR;                   // S1
00835                obs[RinexObsHeader::S1] = rd;
00836             }
00837             else {
00838                rd.ssi = rd.lli = 0; rd.data = Ph;
00839                obs[RinexObsHeader::L2] = rd;                         // L2
00840 
00841                rd.ssi = rd.lli = 0; rd.data = Pr;
00842                obs[RinexObsHeader::P2] = rd;                         // P2
00843 
00844                rd.ssi = rd.lli = 0; rd.data = -Doppler;
00845                obs[RinexObsHeader::D2] = rd;                         // D2
00846 
00847                rd.ssi = rd.lli = 0; rd.data = SNR;
00848                obs[RinexObsHeader::S2] = rd;                         // S2
00849             }
00850 
00851          }
00852 
00853       }  // end RGEC record
00854 
00855       else {                                 // all OEM4 obs records
00856 
00857             // header
00858             // Ref OEM4 Manual pg 16
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             // put timetag into rod
00887          rod.time = DayTime(week,double(msecOfWeek)/1000.);
00888          rod.epochFlag = 0;
00889          rod.clockOffset = 0.0;     // don't have it ?
00890 
00891          if(     rectype == RANGE) {
00892             // Ref OEM4 Manual pg 198-201
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                // break out the TrackStatus
00927                // cf. Table 56, pg 199 of OEM4 manual
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); // 0:L1 1:L2
00933                // CodeType is 0CA 1P 2Pcodeless
00934                int CodeType   = int((TrackStatus & 0x03800000L) >> 23);
00935                bool HalfCycle = bool(TrackStatus & 0x10000000L);
00936 
00937                if(!PhaseLock || !CodeLock) continue;        // data is not reliable
00938 
00939                // fill RinexObsData rod
00940                sat = SatID(prn,SatID::systemGPS);
00941                satit = rod.obs.find(sat);          // find the sat
00942                if(satit == rod.obs.end()) {        // not there - add this sat
00943                   RinexObsData::RinexObsTypeMap rotm;
00944                   rod.obs[sat] = rotm;
00945                   rod.numSvs++;
00946                   satit = rod.obs.find(sat);       // now find it
00947                }
00948 
00949                // for convenience, reference the obs data map inside rod
00950                RinexObsData::RinexObsTypeMap& obs = satit->second;
00951                if(Frequency == 0) {       // frequency = L1
00952                   rd.ssi = rd.lli = 0; rd.data = -Ph;
00953                   obs[RinexObsHeader::L1] = rd;                      // L1
00954 
00955                   rd.ssi = rd.lli = 0; rd.data = Pr;
00956                   if(CodeType == 0) obs[RinexObsHeader::C1] = rd;    // C1
00957                   else              obs[RinexObsHeader::P1] = rd;    // P1
00958 
00959                   rd.ssi = rd.lli = 0; rd.data = Doppler;
00960                   obs[RinexObsHeader::D1] = rd;                      // D1
00961 
00962                   rd.ssi = rd.lli = 0; rd.data = SNR;
00963                   obs[RinexObsHeader::S1] = rd;                      // S1
00964                }
00965                else {
00966                   rd.ssi = rd.lli = 0; rd.data = -Ph;
00967                   obs[RinexObsHeader::L2] = rd;                      // L2
00968 
00969                   rd.ssi = rd.lli = 0; rd.data = Pr;
00970                   obs[RinexObsHeader::P2] = rd;                      // P2
00971 
00972                   rd.ssi = rd.lli = 0; rd.data = Doppler;
00973                   obs[RinexObsHeader::D2] = rd;                      // D2
00974 
00975                   rd.ssi = rd.lli = 0; rd.data = SNR;
00976                   obs[RinexObsHeader::S2] = rd;                      // S2
00977                }
00978 
00979             }
00980 
00981          }  // end RANGE record
00982 
00983          else if(rectype == RANGECMP) {
00984             // Ref OEM4 Manual pg 202-203
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                // this is what is in the manual - its wrong
00998                //double Doppler =   double(data[1] & 0x0FFFFFFFL);
00999                // this is not documented in the manual...
01000                //double Doppler =   double(data[1] & 0x000FFFFFL);
01001                //if(data[1] & 0x0FF00000L == 0x0FF00000L) Doppler = -Doppler;
01002                // try this - cf the OEM2 manual and implementation above
01003                double Doppler  = double((data[1] & 0x0FFFFFFFL));
01004                if(data[1] & 0x08000000L)     // 2s complement
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                // the rest are reserved
01016 
01017                // swap bytes
01018                intelToHost(Doppler);
01019                intelToHost(Pr);
01020                intelToHost(Ph);
01021                intelToHost(SdPrCode);  // code - see pg 203 of OEM4 manual
01022                intelToHost(SdPh);
01023                intelToHost(prn);
01024                intelToHost(locktime);
01025                intelToHost(SNR);
01026 
01027                // convert to physical units
01028                Doppler /= 256.;        // Hz
01029                Pr /= 128.;             // m
01030                Ph /= 256.;             // cycles
01031                double SdPr;
01032                switch(SdPrCode) {      // this is just a code
01033                   // ref table on pg 203 of OEM4 manual
01034                   case  0: SdPr =   0.050; break; // m
01035                   case  1: SdPr =   0.075; break; // m
01036                   case  2: SdPr =   0.113; break; // m
01037                   case  3: SdPr =   0.169; break; // m
01038                   case  4: SdPr =   0.253; break; // m
01039                   case  5: SdPr =   0.380; break; // m
01040                   case  6: SdPr =   0.570; break; // m
01041                   case  7: SdPr =   0.854; break; // m
01042                   case  8: SdPr =   1.281; break; // m
01043                   case  9: SdPr =   2.375; break; // m
01044                   case 10: SdPr =   4.750; break; // m
01045                   case 11: SdPr =   9.500; break; // m
01046                   case 12: SdPr =  19.000; break; // m
01047                   case 13: SdPr =  38.000; break; // m
01048                   case 14: SdPr =  76.000; break; // m
01049                   case 15: SdPr = 152.000; break; // m
01050                   default: SdPr =    0.00; break;
01051                }
01052                SdPh = (SdPh + 1)/512.; // cycles
01053                locktime /= 32.;        // seconds
01054                SNR += 20.;             // dB-Hz
01055                // NB SNR 51 means >=51, and 20 means <=20.
01056 
01057                // break out the TrackStatus
01058                // cf. Table 56, pg 199 of OEM4 manual
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); // 0:L1 1:L2
01064                // CodeType is 0CA 1P 2Pcodeless
01065                int CodeType   = int((TrackStatus & 0x03800000L) >> 23);
01066                bool HalfCycle = bool(TrackStatus & 0x10000000L);
01067 
01068                if(!PhaseLock || !CodeLock) continue;        // data is not reliable
01069 
01070                // correct the phase for rollovers
01071                // cf. OEM4 manual pg 203
01072                double ADRrolls = ((Pr/(Frequency==0 ? wl1 : wl2)) + Ph)/PhaseRollover;
01073                Ph -= long(ADRrolls + (ADRrolls > 0 ? 0.5 : -0.5)) * PhaseRollover;
01074 
01075                // consider debiasing the phase
01076 
01077                // use track status flags to set lli on the phase
01078 
01079                // what to do with HalfCycle?
01080 
01081                // fill RinexObsData rod
01082                sat = SatID(prn,SatID::systemGPS);
01083                satit = rod.obs.find(sat);          // find the sat
01084                if(satit == rod.obs.end()) {        // not there - add this sat
01085                   RinexObsData::RinexObsTypeMap rotm;
01086                   rod.obs[sat] = rotm;
01087                   rod.numSvs++;
01088                   satit = rod.obs.find(sat);       // now find it
01089                }
01090 
01091                // for convenience, reference the obs data map inside rod
01092                RinexObsData::RinexObsTypeMap& obs = satit->second;
01093                if(Frequency == 0) {       // frequency = L1
01094                   rd.ssi = rd.lli = 0; rd.data = -Ph;
01095                   obs[RinexObsHeader::L1] = rd;                      // L1
01096 
01097                   rd.ssi = rd.lli = 0; rd.data = Pr;
01098                   if(CodeType == 0) obs[RinexObsHeader::C1] = rd;    // C1
01099                   else              obs[RinexObsHeader::P1] = rd;    // P1
01100 
01101                   rd.ssi = rd.lli = 0; rd.data = Doppler;
01102                   obs[RinexObsHeader::D1] = rd;                      // D1
01103 
01104                   rd.ssi = rd.lli = 0; rd.data = SNR;
01105                   obs[RinexObsHeader::S1] = rd;                      // S1
01106                }
01107                else {
01108                   rd.ssi = rd.lli = 0; rd.data = -Ph;
01109                   obs[RinexObsHeader::L2] = rd;                      // L2
01110 
01111                   rd.ssi = rd.lli = 0; rd.data = Pr;
01112                   obs[RinexObsHeader::P2] = rd;                      // P2
01113 
01114                   rd.ssi = rd.lli = 0; rd.data = Doppler;
01115                   obs[RinexObsHeader::D2] = rd;                      // D2
01116 
01117                   rd.ssi = rd.lli = 0; rd.data = SNR;
01118                   obs[RinexObsHeader::S2] = rd;                      // S2
01119                }
01120 
01121             }  // end loop over obs
01122 
01123          }  // end RANGECMP record
01124 
01125       }  // end all OEM4 obs records
01126 
01127       return rod;
01128 
01129    }  // end NovatelData::operator RinexObsData()
01130 
01131 }  // end namespace gpstk

Generated on Wed Feb 8 03:31:01 2012 for GPS ToolKit Software Library by  doxygen 1.3.9.1