Changeset 6703 in ntrip for trunk/BNC/src

Timestamp:

Mar 23, 2015, 10:26:42 AM (10 years ago)

Author:

stuerze

Message:

bugfix in RTCM2Decoder

File:

• trunk/BNC/src/RTCM/RTCM2Decoder.cpp (modified) (14 diffs)

trunk/BNC/src/RTCM/RTCM2Decoder.cpp

@@ -66 +66 @@
 }
 
-
 //
 t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz) {
-  if ( !_msg03.validMsg ) {
+  if (!_msg03.validMsg) {
     return failure;
   }
-  
+
   xx = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
   yy = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
@@ -81 +80 @@
 
 //
-t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz,
-                              double& dx1, double& dy1, double& dz1,
-                              double& dx2, double& dy2, double& dz2) {
+t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz, double& dx1,
+    double& dy1, double& dz1, double& dx2, double& dy2, double& dz2) {
   xx = _msg03.x;
   yy = _msg03.y;
@@ -99 +97 @@
 }
 
-
 //
 t_irc RTCM2Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
@@ -106 +103 @@
 
   _buffer.append(buffer, bufLen);
-  int    refWeek;
+  int refWeek;
   double refSecs;
   currentGPSWeeks(refWeek, refSecs);
   bool decoded = false;
 
-  while(true) {
+  while (true) {
     _PP.getPacket(_buffer);
     if (!_PP.valid()) {
@@ -120 +117 @@
       }
     }
-    
+
     // Store message number
     _typeList.push_back(_PP.ID());
 
-    if ( _PP.ID()==18 || _PP.ID()==19 ) {   
+    if (_PP.ID() == 18 || _PP.ID() == 19) {
 
       _ObsBlock.extract(_PP);
@@ -131 +128 @@
         decoded = true;
 
-        int    epochWeek;
+        int epochWeek;
         double epochSecs;
         _ObsBlock.resolveEpoch(refWeek, refSecs, epochWeek, epochSecs);
-          
-        for (int iSat=0; iSat < _ObsBlock.nSat; iSat++) {
+
+        for (int iSat = 0; iSat < _ObsBlock.nSat; iSat++) {
           t_satObs obs;
           if (_ObsBlock.PRN[iSat] > 100) {
             obs._prn.set('R', _ObsBlock.PRN[iSat] % 100);
-          }                     
-          else {
+          } else {
             obs._prn.set('G', _ObsBlock.PRN[iSat]);
-          }                     
+          }
           char sys = obs._prn.system();
           obs._time.set(epochWeek, epochSecs);
+
           t_frqObs* frqObs1C = new t_frqObs;
-          frqObs1C->_rnxType2ch  = "1C";
-          frqObs1C->_codeValid   = true;
-          frqObs1C->_code        = _ObsBlock.rng_C1[iSat];
+          frqObs1C->_rnxType2ch = "1C";
+          frqObs1C->_codeValid = true;
+          frqObs1C->_code = _ObsBlock.rng_C1[iSat];
           obs._obs.push_back(frqObs1C);
 
           t_frqObs* frqObs1P = new t_frqObs;
-          frqObs1P->_rnxType2ch  = (sys == 'G') ? "1W" : "1P";
-          frqObs1P->_codeValid   = true;
-          frqObs1P->_code        = _ObsBlock.rng_P1[iSat];
-          obs._obs.push_back(frqObs1P);
-          frqObs1P->_phaseValid  = true;
-          frqObs1P->_phase       = _ObsBlock.resolvedPhase_L1(iSat);
+          frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
+          frqObs1P->_codeValid = true;
+          frqObs1P->_code = _ObsBlock.rng_P1[iSat];
+          frqObs1P->_phaseValid = true;
+          frqObs1P->_phase = _ObsBlock.resolvedPhase_L1(iSat);
           frqObs1P->_slipCounter = _ObsBlock.slip_L1[iSat];
           obs._obs.push_back(frqObs1P);
 
           t_frqObs* frqObs2P = new t_frqObs;
-          frqObs2P->_rnxType2ch  = (sys == 'G') ? "2W" : "2P";
-          frqObs2P->_codeValid   = true;
-          frqObs2P->_code        = _ObsBlock.rng_P2[iSat];
-          obs._obs.push_back(frqObs2P);
-          frqObs2P->_phaseValid  = true;
-          frqObs2P->_phase       = _ObsBlock.resolvedPhase_L2(iSat);
+          frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
+          frqObs2P->_codeValid = true;
+          frqObs2P->_code = _ObsBlock.rng_P2[iSat];
+          frqObs2P->_phaseValid = true;
+          frqObs2P->_phase = _ObsBlock.resolvedPhase_L2(iSat);
           frqObs2P->_slipCounter = _ObsBlock.slip_L2[iSat];
           obs._obs.push_back(frqObs2P);
@@ -177 +172 @@
     }
 
-    else if ( _PP.ID() == 20 || _PP.ID() == 21 ) {
+    else if (_PP.ID() == 20 || _PP.ID() == 21) {
       _msg2021.extract(_PP);
 
       if (_msg2021.valid()) {
         decoded = true;
-        translateCorr2Obs(errmsg);
-      } 
-    }
-
-    else if ( _PP.ID() == 3 ) {
+        translateCorr2Obs(errmsg);
+      }
+    }
+
+    else if (_PP.ID() == 3) {
       _msg03.extract(_PP);
     }
 
-    else if ( _PP.ID() == 22 ) {
+    else if (_PP.ID() == 22) {
       _msg22.extract(_PP);
     }
 
-    else if ( _PP.ID() == 23 ) {
+    else if (_PP.ID() == 23) {
       _msg23.extract(_PP);
     }
 
-    else if ( _PP.ID() == 24 ) {
+    else if (_PP.ID() == 24) {
       _msg24.extract(_PP);
     }
 
     // Output for RTCM scan
-    if     ( _PP.ID() == 3 ) {
-      if ( _msg03.validMsg ) {
-        _antList.push_back(t_antInfo());
-        
-        this->getStaCrd(_antList.back().xx, _antList.back().yy, _antList.back().zz);
-        
-        _antList.back().type     = t_antInfo::APC;
-        _antList.back().message  = _PP.ID();
-      }
-    }
-    else if ( _PP.ID() == 23 ) {
-      if ( _msg23.validMsg ) {
-        _antType.push_back(_msg23.antType.c_str());
-      }
-    }
-    else if ( _PP.ID() == 24 ) {
-      if ( _msg24.validMsg ) {
-        _antList.push_back(t_antInfo());
-        
-        _antList.back().xx = _msg24.x;
-        _antList.back().yy = _msg24.y;
-        _antList.back().zz = _msg24.z;
-
-        _antList.back().height_f = true;
-        _antList.back().height   = _msg24.h;
-        
-        _antList.back().type     = t_antInfo::ARP;
-        _antList.back().message  = _PP.ID();
+    if (_PP.ID() == 3) {
+      if (_msg03.validMsg) {
+        _antList.push_back(t_antInfo());
+
+        this->getStaCrd(_antList.back().xx, _antList.back().yy,
+            _antList.back().zz);
+
+        _antList.back().type = t_antInfo::APC;
+        _antList.back().message = _PP.ID();
+      }
+    } else if (_PP.ID() == 23) {
+      if (_msg23.validMsg) {
+        _antType.push_back(_msg23.antType.c_str());
+      }
+    } else if (_PP.ID() == 24) {
+      if (_msg24.validMsg) {
+        _antList.push_back(t_antInfo());
+
+        _antList.back().xx = _msg24.x;
+        _antList.back().yy = _msg24.y;
+        _antList.back().zz = _msg24.z;
+
+        _antList.back().height_f = true;
+        _antList.back().height = _msg24.h;
+
+        _antList.back().type = t_antInfo::ARP;
+        _antList.back().message = _PP.ID();
       }
     }
@@ -241 +235 @@
   QMutexLocker locker(&_mutex);
 
-  if ( !_msg03.validMsg || !_msg2021.valid() ) {
+  if (!_msg03.validMsg || !_msg2021.valid()) {
     return;
   }
@@ -249 +243 @@
   double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
 
-  int    refWeek;
+  int refWeek;
   double refSecs;
   currentGPSWeeks(refWeek, refSecs);
@@ -255 +249 @@
   // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
   // ----------------------------------------------------------------------------------
-  double hoursec_est  = _msg2021.hoursec();              // estimated time of measurement
-  double hoursec_rcv  = rint(hoursec_est * 1e2) / 1e2;   // receiver clock reading at hoursec_est  
+  double hoursec_est = _msg2021.hoursec();      // estimated time of measurement
+  double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est
   double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;
 
-  int    GPSWeek;
+  int GPSWeek;
   double GPSWeeks;
-  resolveEpoch(hoursec_est, refWeek, refSecs,
-               GPSWeek, GPSWeeks);
-
-  int    GPSWeek_rcv;
+  resolveEpoch(hoursec_est, refWeek, refSecs, GPSWeek, GPSWeeks);
+
+  int GPSWeek_rcv;
   double GPSWeeks_rcv;
-  resolveEpoch(hoursec_rcv, refWeek, refSecs,
-               GPSWeek_rcv, GPSWeeks_rcv);
+  resolveEpoch(hoursec_rcv, refWeek, refSecs, GPSWeek_rcv, GPSWeeks_rcv);
 
   // Loop over all satellites
   // ------------------------
   for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
-       icorr != _msg2021.data.end(); icorr++) {
+      icorr != _msg2021.data.end(); icorr++) {
     const RTCM2_2021::HiResCorr* corr = icorr->second;
 
     // beg test
-    if ( corr->PRN >= 200 ) {
+    if (corr->PRN >= 200) {
       continue;
     }
@@ -282 +274 @@
 
     QString prn;
-    char    sys;
+    char sys;
     if (corr->PRN < 200) {
       sys = 'G';
       prn = sys + QString("%1").arg(corr->PRN, 2, 10, QChar('0'));
-    }
-    else {
+    } else {
       sys = 'R';
       prn = sys + QString("%1").arg(corr->PRN - 200, 2, 10, QChar('0'));
@@ -302 +293 @@
 
     t_frqObs* frqObs1C = new t_frqObs;
-    frqObs1C->_rnxType2ch  = "1C";
+    frqObs1C->_rnxType2ch = "1C";
     new_obs->_obs.push_back(frqObs1C);
 
     t_frqObs* frqObs1P = new t_frqObs;
-    frqObs1P->_rnxType2ch  = (sys == 'G') ? "1W" : "1P";
+    frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
     new_obs->_obs.push_back(frqObs1P);
 
     t_frqObs* frqObs2P = new t_frqObs;
-    frqObs2P->_rnxType2ch  = (sys == 'G') ? "2W" : "2P";
+    frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
     new_obs->_obs.push_back(frqObs2P);
 
     // missing IOD
     vector<string> missingIOD;
-    vector<string>     hasIOD;
+    vector<string> hasIOD;
     for (unsigned ii = 0; ii < 4; ii++) {
-      int          IODcorr = 0;
-      double       corrVal = 0;
-      const t_eph* eph     = 0;
-      double*      obsVal  = 0;
+      int IODcorr = 0;
+      double corrVal = 0;
+      const t_eph* eph = 0;
+      double* obsVal = 0;
 
       switch (ii) {
-      case 0: // --- L1 ---
-        IODcorr = corr->IODp1;
-        corrVal = corr->phase1 * LAMBDA_1;
-        obsVal  = &L1;
-        obsT    = "L1";
-        break;
-      case 1: // --- L2 ---
-        IODcorr = corr->IODp2;
-        corrVal = corr->phase2 * LAMBDA_2;
-        obsVal  = &L2;
-        obsT    = "L2";
-        break;
-      case 2: // --- P1 ---
-        IODcorr = corr->IODr1;
-        corrVal = corr->range1;
-        obsVal  = &P1;
-        obsT    = "P1";
-        break;
-      case 3: // --- P2 ---
-        IODcorr = corr->IODr2;
-        corrVal = corr->range2;
-        obsVal  = &P2;
-        obsT    = "P2";
-        break;
-      default:
-        continue;
+        case 0: // --- L1 ---
+          IODcorr = corr->IODp1;
+          corrVal = corr->phase1 * LAMBDA_1;
+          obsVal = &L1;
+          obsT = "L1";
+          break;
+        case 1: // --- L2 ---
+          IODcorr = corr->IODp2;
+          corrVal = corr->phase2 * LAMBDA_2;
+          obsVal = &L2;
+          obsT = "L2";
+          break;
+        case 2: // --- P1 ---
+          IODcorr = corr->IODr1;
+          corrVal = corr->range1;
+          obsVal = &P1;
+          obsT = "P1";
+          break;
+        case 3: // --- P2 ---
+          IODcorr = corr->IODr2;
+          corrVal = corr->range2;
+          obsVal = &P2;
+          obsT = "P2";
+          break;
+        default:
+          continue;
       }
 
@@ -354 +345 @@
       const t_eph* ephLast = _ephUser.ephLast(prn);
       const t_eph* ephPrev = _ephUser.ephPrev(prn);
-      if      (ephLast && ephLast->IOD() == IODcorr) {
+      if (ephLast && ephLast->IOD() == IODcorr) {
         eph = ephLast;
-      }
-      else if (ephPrev && ephPrev->IOD() == IODcorr) {
+      } else if (ephPrev && ephPrev->IOD() == IODcorr) {
         eph = ephPrev;
       }
 
-      if ( eph ) {
+      if (eph) {
         ostringstream msg;
         msg << obsT << ':' << setw(3) << eph->IOD();
         hasIOD.push_back(msg.str());
 
-
-        int    GPSWeek_tot;
-        double GPSWeeks_tot;
-        double rho, xSat, ySat, zSat, clkSat;
-        cmpRho(eph, stax, stay, staz, 
-               GPSWeek, GPSWeeks,
-               rho, GPSWeek_tot, GPSWeeks_tot,
-               xSat, ySat, zSat, clkSat);
-
-        *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
-
-        if ( *obsVal == 0 )  *obsVal = ZEROVALUE;
-
-        // Allocate new memory
-        // -------------------
-        if ( !new_obs ) {
-          new_obs = new t_satObs();
+        int GPSWeek_tot;
+        double GPSWeeks_tot;
+        double rho, xSat, ySat, zSat, clkSat;
+        cmpRho(eph, stax, stay, staz, GPSWeek, GPSWeeks, rho, GPSWeek_tot,
+            GPSWeeks_tot, xSat, ySat, zSat, clkSat);
+
+        *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
+
+        if (*obsVal == 0)
+          *obsVal = ZEROVALUE;
+
+        // Allocate new memory
+        // -------------------
+        if (!new_obs) {
+          new_obs = new t_satObs();
           if (corr->PRN < 200) {
             new_obs->_prn.set('G', corr->PRN);
+          } else {
+            new_obs->_prn.set('R', corr->PRN - 200);
           }
-          else {
-            new_obs->_prn.set('R', corr->PRN-200);
-          }
-          new_obs->_time.set(GPSWeek_rcv, GPSWeeks_rcv);
-        }
-        
-        // Store estimated measurements
-        // ----------------------------
-        switch (ii) {
-        case 0: // --- L1 ---
-          frqObs1P->_phaseValid  = true;
-          frqObs1P->_phase       = *obsVal / LAMBDA_1;
-          frqObs1P->_slipCounter = corr->lock1;
-          break;
-        case 1: // --- L2 ---
-          frqObs2P->_phaseValid  = true;
-          frqObs2P->_phase       = *obsVal / LAMBDA_2;
-          frqObs2P->_slipCounter = corr->lock2;
-          break;
-        case 2: // --- C1 / P1 ---
-          if ( corr->Pind1 ) {
-            frqObs1P->_codeValid = true;
-            frqObs1P->_code      = *obsVal;
-          }
-          else {
-            frqObs1C->_codeValid = true;
-            frqObs1C->_code      = *obsVal;
-          }
-          break;
-        case 3: // --- C2 / P2 ---
-          if ( corr->Pind2 ) {
-            frqObs2P->_codeValid = true;
-            frqObs2P->_code      = *obsVal;
-          }
-          break;
-        default:
-          continue;
-        }
-      }
-      else if ( IODcorr != 0 ) {
+          new_obs->_time.set(GPSWeek_rcv, GPSWeeks_rcv);
+        }
+
+        // Store estimated measurements
+        // ----------------------------
+        switch (ii) {
+          case 0: // --- L1 ---
+            frqObs1P->_phaseValid = true;
+            frqObs1P->_phase = *obsVal / LAMBDA_1;
+            frqObs1P->_slipCounter = corr->lock1;
+            break;
+          case 1: // --- L2 ---
+            frqObs2P->_phaseValid = true;
+            frqObs2P->_phase = *obsVal / LAMBDA_2;
+            frqObs2P->_slipCounter = corr->lock2;
+            break;
+          case 2: // --- C1 / P1 ---
+            if (corr->Pind1) {
+              frqObs1P->_codeValid = true;
+              frqObs1P->_code = *obsVal;
+            } else {
+              frqObs1C->_codeValid = true;
+              frqObs1C->_code = *obsVal;
+            }
+            break;
+          case 3: // --- C2 / P2 ---
+            if (corr->Pind2) {
+              frqObs2P->_codeValid = true;
+              frqObs2P->_code = *obsVal;
+            }
+            break;
+          default:
+            continue;
+        }
+      } else if (IODcorr != 0) {
         ostringstream msg;
         msg << obsT << ':' << setw(3) << IODcorr;
@@ -431 +416 @@
       }
     } // loop over frequencies
-   
+
     // Error report
-    if ( missingIOD.size() ) {
+    if (missingIOD.size()) {
       ostringstream missingIODstr;
 
-      copy(missingIOD.begin(), missingIOD.end(), ostream_iterator<string>(missingIODstr, "   "));
-
-      errmsg.push_back("missing eph for " + string(prn.toAscii().data()) + " , IODs " + missingIODstr.str());
+      copy(missingIOD.begin(), missingIOD.end(),
+          ostream_iterator<string>(missingIODstr, "   "));
+
+      errmsg.push_back(
+          "missing eph for " + string(prn.toAscii().data()) + " , IODs "
+              + missingIODstr.str());
     }
 
     // Store new observation
-    if ( new_obs ) {
+    if (new_obs) {
       _obsList.push_back(*new_obs);
       delete new_obs;