discfix.cpp

linux的gps应用

         // loop over sat=it->first, ObsTypeMap=it->second
      for(it=roe.obs.begin(); it != roe.obs.end(); ++it) {

            // Is this satellite excluded ?
         sat = it->first;
         if(sat.system != SatID::systemGPS) continue; // ignore non-GPS satellites
         for(k=-1,i=0; i<config.ExSV.size(); i++)          // ignore user-input sat (--exSat)
            if(config.ExSV[i] == sat) { k = i; break; }
         if(k > -1) continue;

            // if only one satellite is included, skip all the rest
         if(config.SVonly.id != -1 && !(sat == config.SVonly)) continue;

            // pull out the data and the SSI and LLI (indicators)
            // put all the indicators together in a string, then make it a long
            // order of the indicators: P1P2L1L2*ls   AaBbCcDd
         str = string("00000000");
         otmap = it->second;
         if( (jt = otmap.find(rhead.obsTypeList[inP1])) != otmap.end()) {
            spd.P1 = jt->second.data;
            str[0] = (StringUtils::asString(jt->second.lli))[0];
            str[1] = (StringUtils::asString(jt->second.ssi))[0];
         }
         if( (jt = otmap.find(rhead.obsTypeList[inP2])) != otmap.end()) {
            spd.P2 = jt->second.data;
            str[2] = (StringUtils::asString(jt->second.lli))[0];
            str[3] = (StringUtils::asString(jt->second.ssi))[0];
         }
         if( (jt = otmap.find(rhead.obsTypeList[inL1])) != otmap.end()) {
            spd.L1 = jt->second.data;
            str[4] = (StringUtils::asString(jt->second.lli))[0];
            str[5] = (StringUtils::asString(jt->second.ssi))[0];
         }
         if( (jt = otmap.find(rhead.obsTypeList[inL2])) != otmap.end()) {
            spd.L2 = jt->second.data;
            str[6] = (StringUtils::asString(jt->second.lli))[0];
            str[7] = (StringUtils::asString(jt->second.ssi))[0];
         }
         spd.indicators = StringUtils::asUnsigned(str);

            // is it good?
         ok = true;
         if(spd.P1 < 1000.0 || spd.P2 < 1000.0) ok = false;
         if(fabs(spd.L1) <= 0.001 || fabs(spd.L2) <= 0.001) ok = false;
         spd.flag = (ok ? SatPass::OK : SatPass::BAD);

            // process this sat
         ProcessOneSatOneEpoch(sat, CurrEpoch, spd);

      }  // end loop over sats

         // update LastEpoch and estimate of config.dt
      if(config.LastEpoch > DayTime(DayTime::BEGINNING_OF_TIME)) {
         double dt = CurrEpoch-config.LastEpoch;
         for(i=0; i<9; i++) {
            if(config.ndt[i] <=0 ) { config.estdt[i]=dt; config.ndt[i]=1; break; }
            if(fabs(dt-config.estdt[i]) < 0.0001) { config.ndt[i]++; break; }
            if(i == 8) {
               k = 0;
               int nl=config.ndt[k];
               for(j=1; j<9; j++) if(config.ndt[j] <= nl) {
                  k = j;
                  nl = config.ndt[j];
               }
               config.ndt[k] = 1;
               config.estdt[k] = dt;
            }
         }
      }
      config.LastEpoch = CurrEpoch;

         // check times looking for passes that ought to be processed
      for(i=0; i<SPList.size(); i++) {
         if(SPList[i].status > 1)
            continue;                          // already processed
         if(SPList[i].includesTime(CurrEpoch))
            continue;                          // don't process yet

         ProcessSatPass(i);                    // ok, process this pass
         if(!orfstr) SPList[i].status = 99;    // status == 99 means 'written out'
      }

      // try writing more data to output RINEX file
      if(WriteASAP) {
         WriteToRINEXfile();
         // gut passes that have 99
         //for(i=0; i<SPList.size(); i++) {
         //   if(SPList[i].status != 99) continue;
         //   SPList[i].resize(0);
         //}
      }

      return 0;
   }
   catch(Exception& e) { GPSTK_RETHROW(e); }
   catch(exception& e)
      { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
   catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
int ProcessOneSatOneEpoch(RinexSatID sat, DayTime tt, SatPassData& spd)
{
   try {
      int index;
      map<RinexSatID,int>::const_iterator kt;

         // find the current SatPass for this sat
      kt = SatToCurrentIndexMap.find(sat);
         // if there is not one, create one
      if(kt == SatToCurrentIndexMap.end()) {
         SatPass newSP(sat,config.dt);
         SPList.push_back(newSP);
         SPIndexList.push_back(99999);                  // keep parallel
         SatToCurrentIndexMap[sat] = SPList.size()-1;
         kt = SatToCurrentIndexMap.find(sat);
      }

         // update the first epoch
      if(config.FirstEpoch == DayTime::BEGINNING_OF_TIME)
         config.FirstEpoch = CurrEpoch;

         // get the index of this SatPass in the SPList vector
         // and add the data to that SatPass
      index = kt->second;
      SPList[index].status = 1;                // status == 1 means 'fill'
      if( SPList[index].push_back(tt,spd) )
         return 0;

         // --- need to create a new pass ---

         // first process the old one
      ProcessSatPass(index);
      if(!orfstr)                         // not writing to RINEX
         SPList[index].status = 99;       // status == 99 means 'written out'
      else if(WriteASAP)
         WriteToRINEXfile();              // try writing out

         // create a new SatPass for this sat
      SatPass newSP(sat,config.dt);
         // add it to the list
      SPList.push_back(newSP);
      SPIndexList.push_back(99999);                  // keep parallel
         // get the new index
      index = SPList.size()-1;
         // and add it to the map
      SatToCurrentIndexMap[sat] = index;
         // add the data
      SPList[index].status = 1;                // status == 1 means 'fill'
      if(! SPList[index].push_back(tt,spd) )
         ; // throw

      return 0;

   }
   catch(Exception& e) { GPSTK_RETHROW(e); }
   catch(exception& e)
      { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
   catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// Process the pass (call DC); if there is an output file, try writing to it.
void ProcessSatPass(int in)
{
   try {
      config.oflog << "Proc " << SPList[in]
         << " at " << CurrEpoch.printf(config.format) << endl;
      //SPList[in].dump(config.oflog,"RAW");

      // remove this SatPass from the SatToCurrentIndexMap map
      SatToCurrentIndexMap.erase(SPList[in].getSat());

      // --------- call DC on this pass -------------------
      vector<string> EditCmds;
      int iret = DiscontinuityCorrector(SPList[in], GDConfig, EditCmds);
      if(iret != 0) {
         SPList[in].status = 100;         // status == 100 means 'failed'
         return;
      }
      SPList[in].status = 2;              // status == 2 means 'processed'.

      // --------- output editing commands ----------------
      for(int i=0; i<EditCmds.size(); i++)
         config.ofout << EditCmds[i] << endl;

      // --------- smooth pseudorange and debias phase ----
      if(config.smooth) {
         SPList[in].smooth(config.smoothPR,config.smoothPH,config.oflog);
         SPList[in].status = 3;           // status == 3 means 'smoothed'.
      }

      // status ==   0 means 'new'
      // status ==   1 means 'still being filled', so status MUST be set to >1 here
      // status ==   2 means 'processed'
      // status ==   3 means 'smoothed'
      // status ==  98 means 'writing out'
      // status ==  99 means 'written out'
      // status == 100 means 'failed'
   }
   catch(Exception& e) { GPSTK_RETHROW(e); }
   catch(exception& e)
      { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
   catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
int AfterReadingFiles(void)
{
   try {
      config.oflog << "After reading files" << endl;

      // compute the estimated data interval and write it out
      for(int i=1; i<9; i++) if(config.ndt[i] > config.ndt[0]) {
         int j = config.ndt[i];            double est = config.estdt[i];
         config.ndt[i] = config.ndt[0];    config.estdt[i] = config.estdt[0];
         config.ndt[0] = j;                config.estdt[0] = est;
      }
      if(config.verbose)
         config.oflog << "Data interval estimated from the data is "
            << config.estdt[0] << " seconds." << endl;

      // process all the passes that have not been processed yet
      for(int i=0; i<SPList.size(); i++) {
         if(SPList[i].status <= 1) {
            ProcessSatPass(i);
            if(!orfstr)                         // not writing out to RINEX
               SPList[i].status = 99;           // status == 99 means 'written out'
         }
      }

      // write out all the (processed) data that has not already been written
      WriteToRINEXfile();

      // print a summary
      PrintSPList(config.oflog,"Fine",SPList,false);

      return 0;
   }
   catch(Exception& e) { GPSTK_RETHROW(e); }
   catch(exception& e)
      { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
   catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
// this will only write out passes for which ProcessSatPass() has been called. It
// could be called anytime, particularly after each call to ProcessSatPass.
void WriteToRINEXfile(void)
{
   try {
      int in,n;
      DayTime targetTime=DayTime::END_OF_TIME;
      static DayTime WriteEpoch(DayTime::BEGINNING_OF_TIME);

      // find all passes that have been newly processed (status > 1 but < 98)
      // mark these passes 'being written out' and initialize the iterator
      for(in=0; in<SPList.size(); in++) {
         if(SPList[in].status > 1 && SPList[in].status < 98) {
            SPList[in].status = 98;       // status == 98 means 'being written out'
            SPIndexList[in] = 0;          // initialize iteration over the data array
         }
      }

      // find the earliest FirstTime of 'non-processed' (status==1) passes
      for(in=0; in<SPList.size(); in++) {
         if(SPList[in].status == 1 && SPList[in].getFirstTime() < targetTime)
            targetTime = SPList[in].getFirstTime();
      }
      // targetTime will == END_OF_TIME, when all passes have been processed

      if(targetTime < DayTime::END_OF_TIME &&
         WriteEpoch == DayTime::BEGINNING_OF_TIME) {
         WriteRINEXheader();

         WriteEpoch = config.FirstEpoch;
      }

      // nothing to do
      if(targetTime <= WriteEpoch)
         return;

      WriteRINEXdata(WriteEpoch,targetTime);
   }
   catch(Exception& e) { GPSTK_RETHROW(e); }
   catch(exception& e)
      { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
   catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
void WriteRINEXheader(void)
{
   try {
      RinexObsHeader rheadout;   

      config.oflog << "Write the output header at "
         << CurrEpoch.printf(config.format) << endl;

         // copy input
      rheadout = rhead;

         // change the obs type list to include only P1(C1) P2 L1 L2
      rheadout.obsTypeList.clear();

      rheadout.obsTypeList.push_back(RinexObsHeader::L1);
      rheadout.obsTypeList.push_back(RinexObsHeader::L2);
      if(UsingCA)
         rheadout.obsTypeList.push_back(RinexObsHeader::C1);
      else
         rheadout.obsTypeList.push_back(RinexObsHeader::P1);
      rheadout.obsTypeList.push_back(RinexObsHeader::P2);

         // fill records in output header
      rheadout.date = PrgmEpoch.printf("%04Y/%02m/%02d %02H:%02M:%02S");
      rheadout.fileProgram = PrgmName;
      if(!config.HDRunby.empty()) rheadout.fileAgency = config.HDRunby;
      if(!config.HDObs.empty()) rheadout.observer = config.HDObs;
      if(!config.HDAgency.empty()) rheadout.agency = config.HDAgency;
      if(!config.HDMarker.empty()) rheadout.markerName = config.HDMarker;
      if(!config.HDNumber.empty()) rheadout.markerNumber = config.HDNumber;
      rheadout.version = 2.1; rheadout.valid |= RinexObsHeader::versionValid;
      rheadout.firstObs = config.FirstEpoch; rheadout.valid
         |= RinexObsHeader::firstTimeValid;
      rheadout.interval = config.dt; rheadout.valid |= RinexObsHeader::intervalValid;
      if(!WriteASAP) {
         rheadout.interval = config.estdt[0];
         rheadout.valid |= RinexObsHeader::intervalValid;
         rheadout.lastObs = config.LastEpoch;
         rheadout.valid |= RinexObsHeader::lastTimeValid;
      }
      if(config.smoothPR)
         rheadout.commentList.push_back(string("Ranges smoothed by ") +
            PrgmName + string(" v.") + PrgmVers.substr(0,4) + string(" ") +
            rheadout.date);
      if(config.smoothPH)
         rheadout.commentList.push_back(string("Phases debiased by ") +
            PrgmName + string(" v.") + PrgmVers.substr(0,4) + string(" ") +
            rheadout.date);
      if(config.smoothPR || config.smoothPH)
         rheadout.valid |= RinexObsHeader::commentValid;
         // invalidate the table
      if(rheadout.valid & RinexObsHeader::numSatsValid)
         rheadout.valid ^= RinexObsHeader::numSatsValid;
      if(rheadout.valid & RinexObsHeader::prnObsValid)
         rheadout.valid ^= RinexObsHeader::prnObsValid;

      orfstr << rheadout;
   }
   catch(Exception& e) { GPSTK_RETHROW(e); }
   catch(exception& e)
      { Exception E("std except: "+string(e.what())); GPSTK_THROW(E); }
   catch(...) { Exception e("Unknown exception"); GPSTK_THROW(e); }
}

//------------------------------------------------------------------------------------
void WriteRINEXdata(DayTime& WriteEpoch, const DayTime targetTime)
{
   try {
      bool first;
      int in,n;
      string str;
      RinexSatID sat;
      RinexObsData roe;
      SatPassData spd;

      // loop over epochs, up to just before targetTime
      do {
            // find the next WriteEpoch = earliest iterator time among the status==98
         first = true;
         for(in=0; in<SPList.size(); in++) {
            if(SPList[in].status != 98)     // status == 98 means 'being written out'
               continue;

            n = SPIndexList[in];   // current iterator index
            if(first || SPList[in].time(n) < WriteEpoch) {