Changeset 6703 in ntrip


Ignore:
Timestamp:
Mar 23, 2015, 10:26:42 AM (7 years ago)
Author:
stuerze
Message:

bugfix in RTCM2Decoder

File:
1 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNC/src/RTCM/RTCM2Decoder.cpp

    r6443 r6703  
    6666}
    6767
    68 
    6968//
    7069t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz) {
    71   if ( !_msg03.validMsg ) {
     70  if (!_msg03.validMsg) {
    7271    return failure;
    7372  }
    74  
     73
    7574  xx = _msg03.x + (_msg22.validMsg ? _msg22.dL1[0] : 0.0);
    7675  yy = _msg03.y + (_msg22.validMsg ? _msg22.dL1[1] : 0.0);
     
    8180
    8281//
    83 t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz,
    84                               double& dx1, double& dy1, double& dz1,
    85                               double& dx2, double& dy2, double& dz2) {
     82t_irc RTCM2Decoder::getStaCrd(double& xx, double& yy, double& zz, double& dx1,
     83    double& dy1, double& dz1, double& dx2, double& dy2, double& dz2) {
    8684  xx = _msg03.x;
    8785  yy = _msg03.y;
     
    9997}
    10098
    101 
    10299//
    103100t_irc RTCM2Decoder::Decode(char* buffer, int bufLen, vector<string>& errmsg) {
     
    106103
    107104  _buffer.append(buffer, bufLen);
    108   int    refWeek;
     105  int refWeek;
    109106  double refSecs;
    110107  currentGPSWeeks(refWeek, refSecs);
    111108  bool decoded = false;
    112109
    113   while(true) {
     110  while (true) {
    114111    _PP.getPacket(_buffer);
    115112    if (!_PP.valid()) {
     
    120117      }
    121118    }
    122    
     119
    123120    // Store message number
    124121    _typeList.push_back(_PP.ID());
    125122
    126     if ( _PP.ID()==18 || _PP.ID()==19 ) {   
     123    if (_PP.ID() == 18 || _PP.ID() == 19) {
    127124
    128125      _ObsBlock.extract(_PP);
     
    131128        decoded = true;
    132129
    133         int    epochWeek;
     130        int epochWeek;
    134131        double epochSecs;
    135132        _ObsBlock.resolveEpoch(refWeek, refSecs, epochWeek, epochSecs);
    136          
    137         for (int iSat=0; iSat < _ObsBlock.nSat; iSat++) {
     133
     134        for (int iSat = 0; iSat < _ObsBlock.nSat; iSat++) {
    138135          t_satObs obs;
    139136          if (_ObsBlock.PRN[iSat] > 100) {
    140137            obs._prn.set('R', _ObsBlock.PRN[iSat] % 100);
    141           }                     
    142           else {
     138          } else {
    143139            obs._prn.set('G', _ObsBlock.PRN[iSat]);
    144           }                     
     140          }
    145141          char sys = obs._prn.system();
    146142          obs._time.set(epochWeek, epochSecs);
     143
    147144          t_frqObs* frqObs1C = new t_frqObs;
    148           frqObs1C->_rnxType2ch  = "1C";
    149           frqObs1C->_codeValid   = true;
    150           frqObs1C->_code        = _ObsBlock.rng_C1[iSat];
     145          frqObs1C->_rnxType2ch = "1C";
     146          frqObs1C->_codeValid = true;
     147          frqObs1C->_code = _ObsBlock.rng_C1[iSat];
    151148          obs._obs.push_back(frqObs1C);
    152149
    153150          t_frqObs* frqObs1P = new t_frqObs;
    154           frqObs1P->_rnxType2ch  = (sys == 'G') ? "1W" : "1P";
    155           frqObs1P->_codeValid   = true;
    156           frqObs1P->_code        = _ObsBlock.rng_P1[iSat];
    157           obs._obs.push_back(frqObs1P);
    158           frqObs1P->_phaseValid  = true;
    159           frqObs1P->_phase       = _ObsBlock.resolvedPhase_L1(iSat);
     151          frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
     152          frqObs1P->_codeValid = true;
     153          frqObs1P->_code = _ObsBlock.rng_P1[iSat];
     154          frqObs1P->_phaseValid = true;
     155          frqObs1P->_phase = _ObsBlock.resolvedPhase_L1(iSat);
    160156          frqObs1P->_slipCounter = _ObsBlock.slip_L1[iSat];
    161157          obs._obs.push_back(frqObs1P);
    162158
    163159          t_frqObs* frqObs2P = new t_frqObs;
    164           frqObs2P->_rnxType2ch  = (sys == 'G') ? "2W" : "2P";
    165           frqObs2P->_codeValid   = true;
    166           frqObs2P->_code        = _ObsBlock.rng_P2[iSat];
    167           obs._obs.push_back(frqObs2P);
    168           frqObs2P->_phaseValid  = true;
    169           frqObs2P->_phase       = _ObsBlock.resolvedPhase_L2(iSat);
     160          frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
     161          frqObs2P->_codeValid = true;
     162          frqObs2P->_code = _ObsBlock.rng_P2[iSat];
     163          frqObs2P->_phaseValid = true;
     164          frqObs2P->_phase = _ObsBlock.resolvedPhase_L2(iSat);
    170165          frqObs2P->_slipCounter = _ObsBlock.slip_L2[iSat];
    171166          obs._obs.push_back(frqObs2P);
     
    177172    }
    178173
    179     else if ( _PP.ID() == 20 || _PP.ID() == 21 ) {
     174    else if (_PP.ID() == 20 || _PP.ID() == 21) {
    180175      _msg2021.extract(_PP);
    181176
    182177      if (_msg2021.valid()) {
    183178        decoded = true;
    184         translateCorr2Obs(errmsg);
    185       } 
    186     }
    187 
    188     else if ( _PP.ID() == 3 ) {
     179        translateCorr2Obs(errmsg);
     180      }
     181    }
     182
     183    else if (_PP.ID() == 3) {
    189184      _msg03.extract(_PP);
    190185    }
    191186
    192     else if ( _PP.ID() == 22 ) {
     187    else if (_PP.ID() == 22) {
    193188      _msg22.extract(_PP);
    194189    }
    195190
    196     else if ( _PP.ID() == 23 ) {
     191    else if (_PP.ID() == 23) {
    197192      _msg23.extract(_PP);
    198193    }
    199194
    200     else if ( _PP.ID() == 24 ) {
     195    else if (_PP.ID() == 24) {
    201196      _msg24.extract(_PP);
    202197    }
    203198
    204199    // Output for RTCM scan
    205     if     ( _PP.ID() == 3 ) {
    206       if ( _msg03.validMsg ) {
    207         _antList.push_back(t_antInfo());
    208        
    209         this->getStaCrd(_antList.back().xx, _antList.back().yy, _antList.back().zz);
    210        
    211         _antList.back().type     = t_antInfo::APC;
    212         _antList.back().message  = _PP.ID();
    213       }
    214     }
    215     else if ( _PP.ID() == 23 ) {
    216       if ( _msg23.validMsg ) {
    217         _antType.push_back(_msg23.antType.c_str());
    218       }
    219     }
    220     else if ( _PP.ID() == 24 ) {
    221       if ( _msg24.validMsg ) {
    222         _antList.push_back(t_antInfo());
    223        
    224         _antList.back().xx = _msg24.x;
    225         _antList.back().yy = _msg24.y;
    226         _antList.back().zz = _msg24.z;
    227 
    228         _antList.back().height_f = true;
    229         _antList.back().height   = _msg24.h;
    230        
    231         _antList.back().type     = t_antInfo::ARP;
    232         _antList.back().message  = _PP.ID();
     200    if (_PP.ID() == 3) {
     201      if (_msg03.validMsg) {
     202        _antList.push_back(t_antInfo());
     203
     204        this->getStaCrd(_antList.back().xx, _antList.back().yy,
     205            _antList.back().zz);
     206
     207        _antList.back().type = t_antInfo::APC;
     208        _antList.back().message = _PP.ID();
     209      }
     210    } else if (_PP.ID() == 23) {
     211      if (_msg23.validMsg) {
     212        _antType.push_back(_msg23.antType.c_str());
     213      }
     214    } else if (_PP.ID() == 24) {
     215      if (_msg24.validMsg) {
     216        _antList.push_back(t_antInfo());
     217
     218        _antList.back().xx = _msg24.x;
     219        _antList.back().yy = _msg24.y;
     220        _antList.back().zz = _msg24.z;
     221
     222        _antList.back().height_f = true;
     223        _antList.back().height = _msg24.h;
     224
     225        _antList.back().type = t_antInfo::ARP;
     226        _antList.back().message = _PP.ID();
    233227      }
    234228    }
     
    241235  QMutexLocker locker(&_mutex);
    242236
    243   if ( !_msg03.validMsg || !_msg2021.valid() ) {
     237  if (!_msg03.validMsg || !_msg2021.valid()) {
    244238    return;
    245239  }
     
    249243  double staz = _msg03.z + (_msg22.validMsg ? _msg22.dL1[2] : 0.0);
    250244
    251   int    refWeek;
     245  int refWeek;
    252246  double refSecs;
    253247  currentGPSWeeks(refWeek, refSecs);
     
    255249  // Resolve receiver time of measurement (see RTCM 2.3, page 4-42, Message 18, Note 1)
    256250  // ----------------------------------------------------------------------------------
    257   double hoursec_est  = _msg2021.hoursec();              // estimated time of measurement
    258   double hoursec_rcv  = rint(hoursec_est * 1e2) / 1e2;   // receiver clock reading at hoursec_est 
     251  double hoursec_est = _msg2021.hoursec();      // estimated time of measurement
     252  double hoursec_rcv = rint(hoursec_est * 1e2) / 1e2; // receiver clock reading at hoursec_est
    259253  double rcv_clk_bias = (hoursec_est - hoursec_rcv) * c_light;
    260254
    261   int    GPSWeek;
     255  int GPSWeek;
    262256  double GPSWeeks;
    263   resolveEpoch(hoursec_est, refWeek, refSecs,
    264                GPSWeek, GPSWeeks);
    265 
    266   int    GPSWeek_rcv;
     257  resolveEpoch(hoursec_est, refWeek, refSecs, GPSWeek, GPSWeeks);
     258
     259  int GPSWeek_rcv;
    267260  double GPSWeeks_rcv;
    268   resolveEpoch(hoursec_rcv, refWeek, refSecs,
    269                GPSWeek_rcv, GPSWeeks_rcv);
     261  resolveEpoch(hoursec_rcv, refWeek, refSecs, GPSWeek_rcv, GPSWeeks_rcv);
    270262
    271263  // Loop over all satellites
    272264  // ------------------------
    273265  for (RTCM2_2021::data_iterator icorr = _msg2021.data.begin();
    274        icorr != _msg2021.data.end(); icorr++) {
     266      icorr != _msg2021.data.end(); icorr++) {
    275267    const RTCM2_2021::HiResCorr* corr = icorr->second;
    276268
    277269    // beg test
    278     if ( corr->PRN >= 200 ) {
     270    if (corr->PRN >= 200) {
    279271      continue;
    280272    }
     
    282274
    283275    QString prn;
    284     char    sys;
     276    char sys;
    285277    if (corr->PRN < 200) {
    286278      sys = 'G';
    287279      prn = sys + QString("%1").arg(corr->PRN, 2, 10, QChar('0'));
    288     }
    289     else {
     280    } else {
    290281      sys = 'R';
    291282      prn = sys + QString("%1").arg(corr->PRN - 200, 2, 10, QChar('0'));
     
    302293
    303294    t_frqObs* frqObs1C = new t_frqObs;
    304     frqObs1C->_rnxType2ch  = "1C";
     295    frqObs1C->_rnxType2ch = "1C";
    305296    new_obs->_obs.push_back(frqObs1C);
    306297
    307298    t_frqObs* frqObs1P = new t_frqObs;
    308     frqObs1P->_rnxType2ch  = (sys == 'G') ? "1W" : "1P";
     299    frqObs1P->_rnxType2ch = (sys == 'G') ? "1W" : "1P";
    309300    new_obs->_obs.push_back(frqObs1P);
    310301
    311302    t_frqObs* frqObs2P = new t_frqObs;
    312     frqObs2P->_rnxType2ch  = (sys == 'G') ? "2W" : "2P";
     303    frqObs2P->_rnxType2ch = (sys == 'G') ? "2W" : "2P";
    313304    new_obs->_obs.push_back(frqObs2P);
    314305
    315306    // missing IOD
    316307    vector<string> missingIOD;
    317     vector<string>     hasIOD;
     308    vector<string> hasIOD;
    318309    for (unsigned ii = 0; ii < 4; ii++) {
    319       int          IODcorr = 0;
    320       double       corrVal = 0;
    321       const t_eph* eph     = 0;
    322       double*      obsVal = 0;
     310      int IODcorr = 0;
     311      double corrVal = 0;
     312      const t_eph* eph = 0;
     313      double* obsVal = 0;
    323314
    324315      switch (ii) {
    325       case 0: // --- L1 ---
    326         IODcorr = corr->IODp1;
    327         corrVal = corr->phase1 * LAMBDA_1;
    328         obsVal = &L1;
    329         obsT    = "L1";
    330         break;
    331       case 1: // --- L2 ---
    332         IODcorr = corr->IODp2;
    333         corrVal = corr->phase2 * LAMBDA_2;
    334         obsVal = &L2;
    335         obsT    = "L2";
    336         break;
    337       case 2: // --- P1 ---
    338         IODcorr = corr->IODr1;
    339         corrVal = corr->range1;
    340         obsVal = &P1;
    341         obsT    = "P1";
    342         break;
    343       case 3: // --- P2 ---
    344         IODcorr = corr->IODr2;
    345         corrVal = corr->range2;
    346         obsVal = &P2;
    347         obsT    = "P2";
    348         break;
    349       default:
    350         continue;
     316        case 0: // --- L1 ---
     317          IODcorr = corr->IODp1;
     318          corrVal = corr->phase1 * LAMBDA_1;
     319          obsVal = &L1;
     320          obsT = "L1";
     321          break;
     322        case 1: // --- L2 ---
     323          IODcorr = corr->IODp2;
     324          corrVal = corr->phase2 * LAMBDA_2;
     325          obsVal = &L2;
     326          obsT = "L2";
     327          break;
     328        case 2: // --- P1 ---
     329          IODcorr = corr->IODr1;
     330          corrVal = corr->range1;
     331          obsVal = &P1;
     332          obsT = "P1";
     333          break;
     334        case 3: // --- P2 ---
     335          IODcorr = corr->IODr2;
     336          corrVal = corr->range2;
     337          obsVal = &P2;
     338          obsT = "P2";
     339          break;
     340        default:
     341          continue;
    351342      }
    352343
     
    354345      const t_eph* ephLast = _ephUser.ephLast(prn);
    355346      const t_eph* ephPrev = _ephUser.ephPrev(prn);
    356       if      (ephLast && ephLast->IOD() == IODcorr) {
     347      if (ephLast && ephLast->IOD() == IODcorr) {
    357348        eph = ephLast;
    358       }
    359       else if (ephPrev && ephPrev->IOD() == IODcorr) {
     349      } else if (ephPrev && ephPrev->IOD() == IODcorr) {
    360350        eph = ephPrev;
    361351      }
    362352
    363       if ( eph ) {
     353      if (eph) {
    364354        ostringstream msg;
    365355        msg << obsT << ':' << setw(3) << eph->IOD();
    366356        hasIOD.push_back(msg.str());
    367357
    368 
    369         int    GPSWeek_tot;
    370         double GPSWeeks_tot;
    371         double rho, xSat, ySat, zSat, clkSat;
    372         cmpRho(eph, stax, stay, staz,
    373                GPSWeek, GPSWeeks,
    374                rho, GPSWeek_tot, GPSWeeks_tot,
    375                xSat, ySat, zSat, clkSat);
    376 
    377         *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
    378 
    379         if ( *obsVal == 0 )  *obsVal = ZEROVALUE;
    380 
    381         // Allocate new memory
    382         // -------------------
    383         if ( !new_obs ) {
    384           new_obs = new t_satObs();
     358        int GPSWeek_tot;
     359        double GPSWeeks_tot;
     360        double rho, xSat, ySat, zSat, clkSat;
     361        cmpRho(eph, stax, stay, staz, GPSWeek, GPSWeeks, rho, GPSWeek_tot,
     362            GPSWeeks_tot, xSat, ySat, zSat, clkSat);
     363
     364        *obsVal = rho - corrVal + rcv_clk_bias - clkSat;
     365
     366        if (*obsVal == 0)
     367          *obsVal = ZEROVALUE;
     368
     369        // Allocate new memory
     370        // -------------------
     371        if (!new_obs) {
     372          new_obs = new t_satObs();
    385373          if (corr->PRN < 200) {
    386374            new_obs->_prn.set('G', corr->PRN);
     375          } else {
     376            new_obs->_prn.set('R', corr->PRN - 200);
    387377          }
    388           else {
    389             new_obs->_prn.set('R', corr->PRN-200);
    390           }
    391           new_obs->_time.set(GPSWeek_rcv, GPSWeeks_rcv);
    392         }
    393        
    394         // Store estimated measurements
    395         // ----------------------------
    396         switch (ii) {
    397         case 0: // --- L1 ---
    398           frqObs1P->_phaseValid  = true;
    399           frqObs1P->_phase       = *obsVal / LAMBDA_1;
    400           frqObs1P->_slipCounter = corr->lock1;
    401           break;
    402         case 1: // --- L2 ---
    403           frqObs2P->_phaseValid  = true;
    404           frqObs2P->_phase       = *obsVal / LAMBDA_2;
    405           frqObs2P->_slipCounter = corr->lock2;
    406           break;
    407         case 2: // --- C1 / P1 ---
    408           if ( corr->Pind1 ) {
    409             frqObs1P->_codeValid = true;
    410             frqObs1P->_code      = *obsVal;
    411           }
    412           else {
    413             frqObs1C->_codeValid = true;
    414             frqObs1C->_code      = *obsVal;
    415           }
    416           break;
    417         case 3: // --- C2 / P2 ---
    418           if ( corr->Pind2 ) {
    419             frqObs2P->_codeValid = true;
    420             frqObs2P->_code      = *obsVal;
    421           }
    422           break;
    423         default:
    424           continue;
    425         }
    426       }
    427       else if ( IODcorr != 0 ) {
     378          new_obs->_time.set(GPSWeek_rcv, GPSWeeks_rcv);
     379        }
     380
     381        // Store estimated measurements
     382        // ----------------------------
     383        switch (ii) {
     384          case 0: // --- L1 ---
     385            frqObs1P->_phaseValid = true;
     386            frqObs1P->_phase = *obsVal / LAMBDA_1;
     387            frqObs1P->_slipCounter = corr->lock1;
     388            break;
     389          case 1: // --- L2 ---
     390            frqObs2P->_phaseValid = true;
     391            frqObs2P->_phase = *obsVal / LAMBDA_2;
     392            frqObs2P->_slipCounter = corr->lock2;
     393            break;
     394          case 2: // --- C1 / P1 ---
     395            if (corr->Pind1) {
     396              frqObs1P->_codeValid = true;
     397              frqObs1P->_code = *obsVal;
     398            } else {
     399              frqObs1C->_codeValid = true;
     400              frqObs1C->_code = *obsVal;
     401            }
     402            break;
     403          case 3: // --- C2 / P2 ---
     404            if (corr->Pind2) {
     405              frqObs2P->_codeValid = true;
     406              frqObs2P->_code = *obsVal;
     407            }
     408            break;
     409          default:
     410            continue;
     411        }
     412      } else if (IODcorr != 0) {
    428413        ostringstream msg;
    429414        msg << obsT << ':' << setw(3) << IODcorr;
     
    431416      }
    432417    } // loop over frequencies
    433    
     418
    434419    // Error report
    435     if ( missingIOD.size() ) {
     420    if (missingIOD.size()) {
    436421      ostringstream missingIODstr;
    437422
    438       copy(missingIOD.begin(), missingIOD.end(), ostream_iterator<string>(missingIODstr, "   "));
    439 
    440       errmsg.push_back("missing eph for " + string(prn.toAscii().data()) + " , IODs " + missingIODstr.str());
     423      copy(missingIOD.begin(), missingIOD.end(),
     424          ostream_iterator<string>(missingIODstr, "   "));
     425
     426      errmsg.push_back(
     427          "missing eph for " + string(prn.toAscii().data()) + " , IODs "
     428              + missingIODstr.str());
    441429    }
    442430
    443431    // Store new observation
    444     if ( new_obs ) {
     432    if (new_obs) {
    445433      _obsList.push_back(*new_obs);
    446434      delete new_obs;
Note: See TracChangeset for help on using the changeset viewer.