Changeset 6155 in ntrip


Ignore:
Timestamp:
Sep 14, 2014, 10:21:12 AM (8 years ago)
Author:
mervart
Message:
 
Location:
trunk/BNC/src/combination
Files:
2 edited

Legend:

Unmodified
Added
Removed
  • trunk/BNC/src/combination/bnccomb.cpp

    r6109 r6155  
    3939// Constructor
    4040////////////////////////////////////////////////////////////////////////////
    41 cmbParam::cmbParam(parType type_, int index_,
    42                    const QString& ac_, const QString& prn_) {
     41bncComb::cmbParam::cmbParam(parType type_, int index_, const QString& ac_, const QString& prn_) {
    4342
    4443  type   = type_;
     
    7372// Destructor
    7473////////////////////////////////////////////////////////////////////////////
    75 cmbParam::~cmbParam() {
     74bncComb::cmbParam::~cmbParam() {
    7675}
    7776
    7877// Partial
    7978////////////////////////////////////////////////////////////////////////////
    80 double cmbParam::partial(const QString& AC_, const QString& prn_) {
     79double bncComb::cmbParam::partial(const QString& AC_, const QString& prn_) {
    8180 
    8281  if      (type == offACgps) {
     
    106105//
    107106////////////////////////////////////////////////////////////////////////////
    108 QString cmbParam::toString() const {
     107QString bncComb::cmbParam::toString() const {
    109108
    110109  QString outStr;
     
    128127// Constructor
    129128////////////////////////////////////////////////////////////////////////////
    130 bncComb::bncComb() {
     129bncComb::bncComb() : bncEphUser(true) {
    131130
    132131  bncSettings settings;
     
    158157  _rtnetDecoder = 0;
    159158
    160   connect(this, SIGNAL(newMessage(QByteArray,bool)),
     159  connect(this,     SIGNAL(newMessage(QByteArray,bool)),
    161160          BNC_CORE, SLOT(slotMessage(const QByteArray,bool)));
    162161
    163162  connect(BNC_CORE, SIGNAL(providerIDChanged(QString)),
    164           this, SLOT(slotProviderIDChanged(QString)));
     163          this,     SLOT(slotProviderIDChanged(QString)));
     164
     165  connect(BNC_CORE, SIGNAL(newOrbCorrections(QList<t_orbCorr>)),
     166          this,     SLOT(slotNewOrbCorrections(QList<t_orbCorr>)));
     167
     168  connect(BNC_CORE, SIGNAL(newClkCorrections(QList<t_clkCorr>)),
     169          this,     SLOT(slotNewClkCorrections(QList<t_clkCorr>)));
    165170
    166171  // Combination Method
     
    263268    _buffer.remove(epoTime);
    264269  }
    265   QMapIterator<QString, cmbCorr*> itOrbCorr(_orbitCorrs);
    266   while (itOrbCorr.hasNext()) {
    267     itOrbCorr.next();
    268     delete itOrbCorr.value();
    269   }
    270 }
    271 
    272 // Read and store one correction line
    273 ////////////////////////////////////////////////////////////////////////////
    274 void bncComb::processCorrLine(const QString& staID, const QString& line) {
     270}
     271
     272// Remember orbit corrections
     273////////////////////////////////////////////////////////////////////////////
     274void bncComb::slotNewOrbCorrections(QList<t_orbCorr> orbCorrections) {
    275275  QMutexLocker locker(&_mutex);
    276276
    277   // Find the AC Name
    278   // ----------------
    279   QString acName;
    280   QListIterator<cmbAC*> icAC(_ACs);
    281   while (icAC.hasNext()) {
    282     cmbAC* AC = icAC.next();
    283     if (AC->mountPoint == staID) {
    284       acName = AC->name;
    285       break;
    286     }
    287   }
    288   if (acName.isEmpty()) {
    289     return;
    290   }
    291 
    292   // Read the Correction
    293   // -------------------
    294   cmbCorr* newCorr = new cmbCorr();
    295   newCorr->acName = acName;
    296   if (!newCorr->readLine(line) == success) {
    297     delete newCorr;
    298     return;
    299   }
    300 
    301   // Check Glonass
    302   // -------------
    303   if (!_useGlonass) {
    304     if (newCorr->prn[0] == 'R') {
     277  for (int ii = 0; ii < orbCorrections.size(); ii++) {
     278    t_orbCorr& orbCorr = orbCorrections[ii];
     279    QString    staID(orbCorr._staID.c_str());
     280    QString    prn(orbCorr._prn.toString().c_str());
     281
     282    // Find/Check the AC Name
     283    // ----------------------
     284    QString acName;
     285    QListIterator<cmbAC*> icAC(_ACs);
     286    while (icAC.hasNext()) {
     287      cmbAC* AC = icAC.next();
     288      if (AC->mountPoint == staID) {
     289        acName = AC->name;
     290        break;
     291      }
     292    }
     293    if (acName.isEmpty()) {
     294      continue;
     295    }
     296
     297    // Store the correction
     298    // --------------------
     299    QMap<t_prn, t_orbCorr>& storage = _orbCorrections[acName];
     300    storage[orbCorr._prn] = orbCorr;
     301  }
     302}
     303
     304// Process clock corrections
     305////////////////////////////////////////////////////////////////////////////
     306void bncComb::slotNewClkCorrections(QList<t_clkCorr> clkCorrections) {
     307  QMutexLocker locker(&_mutex);
     308
     309  bncTime lastTime;
     310
     311  for (int ii = 0; ii < clkCorrections.size(); ii++) {
     312    t_clkCorr& clkCorr = clkCorrections[ii];
     313    QString    staID(clkCorr._staID.c_str());
     314    QString    prn(clkCorr._prn.toString().c_str());
     315
     316    // Set the last time
     317    // -----------------
     318    if (lastTime.undef() || clkCorr._time > lastTime) {
     319      lastTime = clkCorr._time;
     320    }
     321
     322    // Find/Check the AC Name
     323    // ----------------------
     324    QString acName;
     325    QListIterator<cmbAC*> icAC(_ACs);
     326    while (icAC.hasNext()) {
     327      cmbAC* AC = icAC.next();
     328      if (AC->mountPoint == staID) {
     329        acName = AC->name;
     330        break;
     331      }
     332    }
     333    if (acName.isEmpty()) {
     334      continue;
     335    }
     336
     337    // Check GLONASS
     338    // -------------
     339    if (!_useGlonass && clkCorr._prn.system() == 'R') {
     340      continue;
     341    }
     342
     343    // Check Modulo Time
     344    // -----------------
     345    if (int(clkCorr._time.gpssec()) % _cmbSampl != 0.0) {
     346      continue;
     347    }
     348
     349    // Check Correction Age
     350    // --------------------
     351    if (_resTime.valid() && clkCorr._time <= _resTime) {
     352      emit newMessage("bncComb: old correction: " + acName.toAscii() + " " + prn.toAscii(), true);
     353      continue;
     354    }
     355
     356    // Create new correction
     357    // ---------------------
     358    cmbCorr* newCorr  = new cmbCorr();
     359    newCorr->_prn     = clkCorr._prn;
     360    newCorr->_time    = clkCorr._time;
     361    newCorr->_iod     = clkCorr._iod;
     362    newCorr->_acName  = acName;
     363    newCorr->_clkCorr = new t_clkCorr(clkCorr);
     364
     365    // Check the Ephemeris
     366    //--------------------
     367    if (_eph.find(prn) == _eph.end()) {
     368      emit newMessage("bncComb: eph not found "  + prn.toAscii(), true);
    305369      delete newCorr;
    306       return;
    307     }
    308   }
    309 
    310   // Save Orbit-Only Corrections
    311   // ---------------------------
    312   if (newCorr->tRao.valid() && !newCorr->tClk.valid()) {
    313     QString corrID = newCorr->ID();
    314     if (_orbitCorrs.find(corrID) != _orbitCorrs.end()) {
    315       delete _orbitCorrs[corrID];
    316     }
    317     _orbitCorrs[corrID] = newCorr;
    318     return;
    319   }
    320 
    321   // Merge with saved orbit correction
    322   // ---------------------------------
    323   else if (newCorr->tClk.valid() && !newCorr->tRao.valid()) {
    324     QString corrID = newCorr->ID();
    325     if (_orbitCorrs.find(corrID) != _orbitCorrs.end()) {
    326       mergeOrbitCorr(_orbitCorrs[corrID], newCorr);
    327     }
    328   }
    329 
    330   // Check Modulo Time
    331   // -----------------
    332   if (int(newCorr->tClk.gpssec()) % _cmbSampl != 0.0) {
    333     delete newCorr;
    334     return;
    335   }
    336 
    337   // Delete old corrections
    338   // ----------------------
    339   if (_resTime.valid() && newCorr->tClk <= _resTime) {
    340     emit newMessage("bncComb: old correction: " + staID.toAscii() + " " + line.toAscii(), true);
    341     delete newCorr;
    342     return;
    343   }
    344 
    345   // Check the Ephemeris
    346   //--------------------
    347   if (_eph.find(newCorr->prn) == _eph.end()) {
    348     emit newMessage("bncComb: eph not found "  + newCorr->prn.toAscii(), true);
    349     delete newCorr;
    350     return;
    351   }
    352   else {
    353     t_eph* lastEph = _eph[newCorr->prn]->last;
    354     t_eph* prevEph = _eph[newCorr->prn]->prev;
    355     if      (lastEph && lastEph->IOD() == newCorr->iod) {
    356       newCorr->eph = lastEph;
    357     }
    358     else if (lastEph && prevEph && prevEph->IOD() == newCorr->iod) {
    359       newCorr->eph = prevEph;
    360       switchToLastEph(lastEph, newCorr);
     370      continue;
    361371    }
    362372    else {
    363       emit newMessage("bncComb: eph not found "  + newCorr->prn.toAscii() +
    364                       QString(" %1").arg(newCorr->iod).toAscii(), true);
    365       delete newCorr;
    366       return;
    367     }
     373      t_eph* lastEph = _eph[prn]->last;
     374      t_eph* prevEph = _eph[prn]->prev;
     375      if      (lastEph && lastEph->IOD() == newCorr->_iod) {
     376        newCorr->_eph = lastEph;
     377      }
     378      else if (lastEph && prevEph && prevEph->IOD() == newCorr->_iod) {
     379        newCorr->_eph = prevEph;
     380        switchToLastEph(lastEph, newCorr);
     381      }
     382      else {
     383        emit newMessage("bncComb: eph not found "  + prn.toAscii() +
     384                        QString(" %1").arg(newCorr->_iod).toAscii(), true);
     385        delete newCorr;
     386        continue;
     387      }
     388    }
     389
     390    // Store correction into the buffer
     391    // --------------------------------
     392    QVector<cmbCorr*>& corrs = _buffer[newCorr->_time].corrs;
     393    corrs.push_back(newCorr);
    368394  }
    369395
     
    374400  while (itTime.hasNext()) {
    375401    bncTime epoTime = itTime.next();
    376     if (epoTime < newCorr->tClk - waitTime) {
     402    if (epoTime < lastTime - waitTime) {
    377403      _resTime = epoTime;
    378404      processEpoch();
    379405    }
    380406  }
    381 
    382   // Merge or add the correction
    383   // ---------------------------
    384   QVector<cmbCorr*>& corrs = _buffer[newCorr->tClk].corrs;
    385   cmbCorr* existingCorr = 0;
    386   QVectorIterator<cmbCorr*> itCorr(corrs);
    387   while (itCorr.hasNext()) {
    388     cmbCorr* hlp = itCorr.next();
    389     if (hlp->prn == newCorr->prn && hlp->acName == newCorr->acName) {
    390       existingCorr = hlp;
    391       break;
    392     }
    393   }
    394   if (existingCorr) {
    395     delete newCorr; newCorr = 0;
    396     existingCorr->readLine(line); // merge (multiple messages)
    397    
    398   }
    399   else {
    400     corrs.append(newCorr);
    401   }
    402407}
    403408
    404409// Change the correction so that it refers to last received ephemeris
    405410////////////////////////////////////////////////////////////////////////////
    406 void bncComb::switchToLastEph(const t_eph* lastEph, t_corr* corr) {
    407 
    408   if (corr->eph == lastEph) {
     411void bncComb::switchToLastEph(const t_eph* lastEph, cmbCorr* corr) {
     412
     413  if (corr->_eph == lastEph) {
    409414    return;
    410415  }
     
    412417  ColumnVector oldXC(4);
    413418  ColumnVector oldVV(3);
    414   corr->eph->getCrd(corr->tRao, oldXC, oldVV, false);
     419  corr->_eph->getCrd(corr->_time, oldXC, oldVV, false);
    415420
    416421  ColumnVector newXC(4);
    417422  ColumnVector newVV(3);
    418   lastEph->getCrd(corr->tRao, newXC, newVV, false);
     423  lastEph->getCrd(corr->_time, newXC, newVV, false);
    419424
    420425  ColumnVector dX = newXC.Rows(1,3) - oldXC.Rows(1,3);
     
    428433  XYZ_to_RSW(newXC.Rows(1,3), newVV, dV, dDotRAO);
    429434
    430   QString msg = "switch corr " + corr->prn
    431     + QString(" %1 -> %2 %3").arg(corr->iod,3)
    432     .arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
     435  QString msg = "switch corr " + corr->_prn
     436    + QString(" %1 -> %2 %3").arg(corr->_iod,3).arg(lastEph->IOD(),3).arg(dC*t_CST::c, 8, 'f', 4);
    433437
    434438  emit newMessage(msg.toAscii(), false);
    435439
    436   corr->iod     = lastEph->IOD();
    437   corr->eph     = lastEph;
    438   corr->rao    += dRAO;
    439   corr->dotRao += dDotRAO;
    440   corr->dClk   -= dC;
     440  corr->_iod = lastEph->IOD();
     441  corr->_eph = lastEph;
     442
     443  if (corr->_clkCorr) {
     444    corr->_clkCorr->_dClk -= dC;
     445  }
     446  if (corr->_orbCorr) {
     447    for (int ii = 0; ii < 3; ii++) {
     448      corr->_orbCorr->_xr[ii]    += dRAO[ii];
     449      corr->_orbCorr->_dotXr[ii] += dDotRAO[ii];
     450    }
     451  }
    441452}
    442453
  • trunk/BNC/src/combination/bnccomb.h

    r6154 r6155  
    2121 public slots:
    2222  void slotProviderIDChanged(QString mountPoint);
    23   void slotNewOrbCorrections(QList<t_orbCorr> orbCorr);
    24   void slotNewClkCorrections(QList<t_clkCorr> clkCorr);
     23  void slotNewOrbCorrections(QList<t_orbCorr> orbCorrections);
     24  void slotNewClkCorrections(QList<t_clkCorr> clkCorrections);
    2525
    2626 signals:
    2727  void newMessage(QByteArray msg, bool showOnScreen);
    28   void newClkCorrections(QList<t_clkCorr> clkCorr);
     28  void newClkCorrections(QList<t_clkCorr> clkCorrections);
     29  void newOrbCorrections(QList<t_orbCorr> orbCorrections);
    2930
    3031 private:
     
    6263  };
    6364
    64   class cmbCorr : public t_clkCorr {
     65  class cmbCorr {
    6566   public:
    66     QString      acName;
    67     ColumnVector diffRao;
    68     QString ID() {return acName + "_" + QString(_prn.toString().c_str());}
     67    cmbCorr() {
     68      _eph     = 0;
     69      _iod     = 0;
     70      _orbCorr = 0;
     71      _clkCorr = 0;
     72    }
     73    ~cmbCorr() {
     74      delete _orbCorr;
     75      delete _clkCorr;
     76    }
     77    t_prn        _prn;
     78    bncTime      _time;
     79    int          _iod;
     80    const t_eph* _eph;
     81    t_orbCorr*   _orbCorr;
     82    t_clkCorr*   _clkCorr;
     83    QString      _acName;
     84    ColumnVector _diffRao;
     85    QString ID() {return _acName + "_" + QString(_prn.toString().c_str());}
    6986  };
    7087
     
    8299
    83100  void  processEpoch();
    84   t_irc processEpoch_filter(QTextStream& out, QMap<QString, t_clkCorr*>& resCorr,
     101  t_irc processEpoch_filter(QTextStream& out, QMap<QString, cmbCorr*>& resCorr,
    85102                            ColumnVector& dx);
    86   t_irc processEpoch_singleEpoch(QTextStream& out, QMap<QString, t_clkCorr*>& resCorr,
     103  t_irc processEpoch_singleEpoch(QTextStream& out, QMap<QString, cmbCorr*>& resCorr,
    87104                                 ColumnVector& dx);
    88105  t_irc createAmat(Matrix& AA, ColumnVector& ll, DiagonalMatrix& PP,
    89                    const ColumnVector& x0, QMap<QString, t_clkCorr*>& resCorr);
    90   void  dumpResults(const QMap<QString, t_clkCorr*>& resCorr);
    91   void  printResults(QTextStream& out, const QMap<QString, t_clkCorr*>& resCorr);
    92   void  switchToLastEph(const t_eph* lastEph, t_clkCorr* corr);
     106                   const ColumnVector& x0, QMap<QString, cmbCorr*>& resCorr);
     107  void  dumpResults(const QMap<QString, cmbCorr*>& resCorr);
     108  void  printResults(QTextStream& out, const QMap<QString, cmbCorr*>& resCorr);
     109  void  switchToLastEph(const t_eph* lastEph, cmbCorr* corr);
    93110  t_irc checkOrbits(QTextStream& out);
    94111  t_irc mergeOrbitCorr(const cmbCorr* orbitCorr, cmbCorr* clkCorr);
    95112
    96   QVector<cmbCorr*>& corrs() {return _buffer[_resTime].corrs;}
    97 
    98   QList<cmbAC*>           _ACs;
    99   bncTime                 _resTime;
    100   QVector<cmbParam*>      _params;
    101   QMap<bncTime, cmbEpoch> _buffer;
    102   bncRtnetDecoder*        _rtnetDecoder;
    103   SymmetricMatrix         _QQ;
    104   QByteArray              _log;
    105   bncAntex*               _antex;
    106   double                  _MAXRES;
    107   QString                 _masterOrbitAC;
    108   unsigned                _masterMissingEpochs;
    109   e_method                _method;
    110   bool                    _useGlonass;
    111   int                     _cmbSampl;
    112   QMap<QString, cmbCorr*> _orbitCorrs;
     113  QList<cmbAC*>                          _ACs;
     114  bncTime                                _resTime;
     115  QVector<cmbParam*>                     _params;
     116  QMap<bncTime, cmbEpoch>                _buffer;
     117  bncRtnetDecoder*                       _rtnetDecoder;
     118  SymmetricMatrix                        _QQ;
     119  QByteArray                             _log;
     120  bncAntex*                              _antex;
     121  double                                 _MAXRES;
     122  QString                                _masterOrbitAC;
     123  unsigned                               _masterMissingEpochs;
     124  e_method                               _method;
     125  bool                                   _useGlonass;
     126  int                                    _cmbSampl;
     127  QMap<QString, QMap<t_prn, t_orbCorr> > _orbCorrections;
    113128};
    114129
Note: See TracChangeset for help on using the changeset viewer.