Changeset 725 in ntrip


Ignore:
Timestamp:
Mar 12, 2008, 10:29:01 AM (14 years ago)
Author:
weber
Message:

* empty log message *

Location:
trunk/BNC/RTCM
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNC/RTCM/RTCM2.cpp

    r711 r725  
    4545//   2008/03/04  AHA  Fixed problems with PRN 32
    4646//   2008/03/05  AHA  Implemeted fix for Trimble 4000SSI receivers
     47//   2008/03/07  AHA  Major revision of input buffer handling
     48//   2008/03/07  AHA  Removed unnecessary failure flag
     49//   2008/03/10  AHA  Corrected extraction of antenna serial number
     50//   2008/03/10  AHA  Corrected buffer length check in getPacket()
     51//   2008/03/11  AHA  Added checks for data consistency in extraction routines
     52//   2008/03/11  AHA  isGPS-flag in RTCM2_Obs is now set to false on clear()
    4753//
    4854// (c) DLR/GSOC
     
    6369// undersized packets in get(Unsigned)Bits
    6470
    65 #define DEBUG 0   
     71#define DEBUG 0
    6672
    6773// Activate (1) or deactivate (0) rounding of measurement epochs to 100ms
     
    115121
    116122namespace rtcm2 {
    117 
    118 
     123 
    119124//------------------------------------------------------------------------------
    120125//
     
    136141void ThirtyBitWord::clear() {
    137142  W = 0;
    138 };
    139 
    140 // Failure indicator for input operations
    141 
    142 bool ThirtyBitWord::fail() const {
    143   return failure;
    144143};
    145144
     
    259258
    260259
    261 
    262260// Append a byte with six data bits
    263261
     
    275273  // Bits 7 and 6 (of 0..7) must be "01" for valid data bytes
    276274  if ( (b & 0x40) != 0x40 ) {
    277     failure = true;
     275    // We simply skip the invalid input byte and leave the word unchanged
     276#if (DEBUG>0)
     277    cerr << "Error in append()" << bitset<32>(all()) << endl;
     278#endif
    278279    return;
    279280  };
     
    290291// Get next 30bit word from string
    291292
    292 void ThirtyBitWord::get(string& buf) {
     293void ThirtyBitWord::get(const string& buf) {
    293294
    294295  // Check if string is long enough
    295296   
    296297  if (buf.size()<5) {
    297     failure = true;
     298    // Ignore; users should avoid this case prior to calling get()   
     299#if ( DEBUG > 0 )   
     300    cerr << "Error in get(): packet too short (" << buf.size() <<")" << endl;
     301#endif
    298302    return;
    299303  };
    300304 
    301   // Process 5 bytes and remove them from the input
     305  // Process 5 bytes
    302306 
    303307  for (int i=0; i<5; i++) append(buf[i]);
    304   buf.erase(0,5);
    305308
    306309#if (DEBUG>0)
    307310  if (!validParity()) {
    308     cerr << "Parity error "
     311    cerr << "Parity error in get()"
    309312         << bitset<32>(all()) << endl;
    310313  };
    311314#endif
    312   failure = false;
    313315
    314316};
     
    328330#if (DEBUG>0)
    329331  if (!validParity()) {
    330     cerr << "Parity error "
     332    cerr << "Parity error in get()"
    331333         << bitset<32>(all()) << endl;
    332334  };
    333335#endif
    334   failure = false;
    335336
    336337};
     
    340341void ThirtyBitWord::getHeader(string& buf) {
    341342
    342   unsigned int W_old = W;
     343  const int wordLen = 5; // Number of bytes representing a 30-bit word
     344  const int spare   = 1; // Number of spare words for resync of parity
     345                         // (same value as inRTCM2packet::getPacket())
    343346  unsigned int i;
    344347 
    345348  i=0;
    346   while (!isHeader() || i<5 ) {
    347     // Check if string is long enough; if not restore old word and exit
    348     if (buf.size()<i+1) {
    349       W = W_old;
    350       failure = true;
    351       return;
    352     };
     349  while (!isHeader() && i<buf.size() ) {
    353350    // Process byte
    354     append(buf[i]); i++;
    355   };
    356 
    357   // Remove processed bytes from buffer
    358  
    359   buf.erase(0,i);
     351    append(buf[i]);
     352    // Increment count
     353    i++;
     354  };
     355
     356  // Remove processed bytes from buffer. Retain also the previous word to
     357  // allow a resync if getHeader() is called repeatedly on the same buffer.
     358  if (i>=(1+spare)*wordLen) buf.erase(0,i-(1+spare)*wordLen);
    360359
    361360#if (DEBUG>0)
    362361  if (!validParity()) {
    363     cerr << "Parity error "
     362    cerr << "Parity error in getHeader()"
    364363         << bitset<32>(all()) << endl;
    365364  };
    366365#endif
    367   failure = false;
    368 
     366 
    369367};
    370368
     
    385383#if (DEBUG>0)
    386384  if (!validParity()) {
    387     cerr << "Parity error "
     385    cerr << "Parity error in getHeader()"
    388386         << bitset<32>(all()) << endl;
    389387  };
    390388#endif
    391   failure = false;
    392389
    393390};
     
    443440void RTCM2packet::getPacket(std::string& buf) {
    444441
    445   int           n;
    446   ThirtyBitWord W_old = W;
    447   string        buf_old = buf;
    448  
    449   // Try to read a full packet. If the input buffer is too short
    450   // clear all data and restore the latest 30-bit word prior to
    451   // the getPacket call. The empty header word will indicate
    452   // an invalid message, which signals an unsuccessful getPacket()
    453   // call.
    454    
    455   W.getHeader(buf);
    456   H1 = W.value();
    457   if (W.fail()) { clear(); W=W_old; buf=buf_old; return; };
    458   if (!W.validParity()) { clear(); return; };
    459  
    460   W.get(buf);       
    461   H2 = W.value();
    462   if (W.fail()) { clear(); W=W_old; buf=buf_old; return; };
    463   if (!W.validParity()) { clear(); return; };
     442  const int wordLen = 5; // Number of bytes representing a 30-bit word
     443  const int spare   = 1; // Number of spare words for resync of parity
     444                         // (same value as used in ThirtyBitWord::getHeader)
     445  unsigned int n;
     446 
     447  // Try to read a full packet. Processed bytes are removed from the input
     448  // buffer except for the latest spare*wordLen bytes to restore the parity
     449  // bytes upon subseqeunt calls of getPAcket().
     450 
     451  // Locate and read the first header word
     452  W.getHeader(buf);
     453  if (!W.isHeader()) {
     454    // No header found; try again next time. buf retains only the spare
     455    // words. The packet contents is cleared to indicate an unsuccessful
     456    // termination of getPacket().
     457    clear();
     458#if ( DEBUG > 0 )
     459    cerr << "Error in getPacket(): W.isHeader() = false  for H1" << endl;
     460#endif
     461    return;
     462  };
     463  H1 = W.value();
     464
     465  // Do we have enough bytes to read the next word? If not, the packet
     466  // contents is cleared to indicate an unsuccessful termination. The
     467  // previously read spare and header bytes are retained in the buffer
     468  // for use in the next call of getPacket().
     469  if (buf.size()<(spare+2)*wordLen) {
     470    clear();
     471#if ( DEBUG > 0 )   
     472    cerr << "Error in getPacket(): buffer too short for complete H2" << endl;
     473#endif
     474    return;
     475  };
     476 
     477  // Read the second header word
     478  W.get(buf.substr((spare+1)*wordLen,buf.size()-(spare+1)*wordLen)); 
     479  H2 = W.value();
     480  if (!W.validParity()) {
     481    // Invalid H2 word; delete first buffer byte and try to resynch next time.
     482    // The packet contents is cleared to indicate an unsuccessful termination.
     483    clear();
     484    buf.erase(0,1);
     485#if ( DEBUG > 0 )   
     486    cerr << "Error in getPacket(): W.validParity() = false for H2" << endl;
     487#endif
     488    return;
     489  };
    464490
    465491  n = nDataWords();
     492
     493  // Do we have enough bytes to read the next word? If not, the packet
     494  // contents is cleared to indicate an unsuccessful termination. The
     495  // previously read spare and header bytes are retained in the buffer
     496  // for use in the next call of getPacket().
     497  if (buf.size()<(spare+2+n)*wordLen) {
     498    clear();     
     499#if ( DEBUG > 0 )   
     500    cerr << "Error in getPacket(): buffer too short for complete " << n
     501         << " DWs" << endl;
     502#endif
     503    return;
     504  };
     505 
    466506  DW.resize(n);
    467   for (int i=0; i<n; i++) {
    468     W.get(buf);
    469     DW[i] = W.value();
    470     if (W.fail()) { clear(); W=W_old; buf=buf_old; return; };
    471     if (!W.validParity()) { clear(); return; };
    472   };
    473 
     507  for (unsigned int i=0; i<n; i++) {
     508    W.get(buf.substr((spare+2+i)*wordLen,buf.size()-(spare+2+i)*wordLen));
     509    DW[i] = W.value();
     510    if (!W.validParity()) {
     511      // Invalid data word; delete first byte and try to resynch next time.
     512      // The packet contents is cleared to indicate an unsuccessful termination.
     513      clear();
     514      buf.erase(0,1);
     515#if ( DEBUG > 0 )   
     516    cerr << "Error in getPacket(): W.validParity() = false for DW"
     517         << i << endl;
     518#endif
     519      return;
     520    };
     521  };
     522
     523  // Successful packet extraction; delete total number of message bytes
     524  // from buffer.
     525  // Note: a total of "spare" words remain in the buffer to enable a
     526  // parity resynchronization when searching the next header.
     527 
     528  buf.erase(0,(n+2)*wordLen);
     529 
    474530  return;
    475531 
     
    487543  W.getHeader(inp);
    488544  H1 = W.value();
    489   if (W.fail() || !W.validParity()) { clear(); return; }
     545  if (inp.fail() || !W.isHeader()) { clear(); return; }
    490546 
    491547  W.get(inp);       
    492548  H2 = W.value();
    493   if (W.fail() || !W.validParity()) { clear(); return; }
     549  if (inp.fail() || !W.validParity()) { clear(); return; }
    494550
    495551  n = nDataWords();
     
    498554    W.get(inp);
    499555    DW[i] = W.value();
    500     if (W.fail() || !W.validParity()) { clear(); return; }
     556    if (inp.fail() || !W.validParity()) { clear(); return; }
    501557  };
    502558
     
    675731void RTCM2_03::extract(const RTCM2packet& P) {
    676732
    677   // Check validity and packet type
     733  // Check validity, packet type and number of data words
    678734 
    679735  validMsg = (P.valid());
     
    681737
    682738  validMsg = (P.ID()==03); 
     739  if (!validMsg) return;
     740 
     741  validMsg = (P.nDataWords()==4); 
    683742  if (!validMsg) return;
    684743 
     
    724783    antSN = "";
    725784    for (int i=0;i<nas;i++)
    726       antSN += (char)P.getUnsignedBits(24+8*nas+i*8,8);
     785      antSN += (char)P.getUnsignedBits(24+8*nad+i*8,8);
    727786  };
    728787
     
    745804   double dx,dy,dz;
    746805
    747   // Check validity and packet type
     806  // Check validity, packet type and number of data words
    748807 
    749808  validMsg = (P.valid());
     
    751810
    752811  validMsg = (P.ID()==24); 
     812  if (!validMsg) return;
     813 
     814  validMsg = (P.nDataWords()==6); 
    753815  if (!validMsg) return;
    754816 
     
    806868
    807869  clear();
     870
     871};
     872
     873// Reset entire block
     874
     875void RTCM2_Obs::clear() {
     876 
    808877  GPSonly = true;
    809 
    810 };
    811 
    812 // Reset entire block
    813 
    814 void RTCM2_Obs::clear() {
    815 
     878 
    816879  secs=0.0;                // Seconds of hour (GPS time)
    817880  nSat=0;                  // Number of space vehicles
    818   PRN.resize(0);           // Pseudorange [m]
     881  PRN.resize(0);           // space vehicles
    819882  rng_C1.resize(0);        // Pseudorange [m]
    820883  rng_P1.resize(0);        // Pseudorange [m]
     
    888951 
    889952  if ( ! ( P.valid() &&
    890            (P.ID()==18 || P.ID()==19) &&
    891            P.nDataWords()>1              ) ) return;
    892 
     953           (P.ID()==18 || P.ID()==19) ) ) return;
     954
     955  // Check number of data words, message starts with 1 DW for epoch, then each
     956  // satellite brings 2 DW,
     957  // Do not start decoding if less than 3 DW are in package
     958 
     959  if ( P.nDataWords()<3 ) {
     960#if ( DEBUG > 0 )
     961    cerr << "Error in RTCM2_Obs::extract(): less than 3 DW ("
     962         << P.nDataWords() << ") detected" << endl;
     963#endif
     964    return;
     965  };
     966 
     967  // Check if number of data words is odd number
     968 
     969  if ( P.nDataWords()%2==0 ){
     970#if ( DEBUG > 0 )
     971    cerr << "Error in RTCM2_Obs::extract(): odd number of DW ("
     972         << P.nDataWords() << ") detected" << endl;
     973#endif
     974    return;
     975  };
     976 
    893977  // Clear previous data if block was already complete
    894978
     
    9491033    if ( isL1 && !isGPS) availability.set(bit_L1cphGLO);
    9501034    if (!isL1 && !isGPS) availability.set(bit_L2cphGLO);
    951 
     1035   
    9521036    // Process all satellites
    9531037   
  • trunk/BNC/RTCM/RTCM2.h

    r711 r725  
    2828//   2006/10/17  OMO  Removed obsolete check of multiple message indicator
    2929//   2006/11/25  OMO  Revised check for presence of GLONASS data
     30//   2008/03/07  AHA  Removed unnecessary failure flag
    3031//
    3132// (c) DLR/GSOC
     
    8182    // Input
    8283   
    83     void         get(std::string& buf);
     84    void         get(const std::string& buf);
    8485    void         get(std::istream& inp);
    8586    void         getHeader(std::string& buf);
     
    9495  private:
    9596
    96     bool         failure;
     97//    bool         failure;
    9798
    9899    //
Note: See TracChangeset for help on using the changeset viewer.