synchronization.cpp

linux的gps应用

               + string(" at count ") + StringUtils::asString(rawdat.count[nc])
               + string(" = time ")
               + (FirstEpoch + rawdat.count[nc]*CI.DataInterval).printf(
                  "%Y/%m/%d %H:%02M:%6.3f = %F/%10.3g"));
            GPSTK_THROW(e);
         }

         // fit a polynomial of degree D to the points in deques
         PF1.Reset(D<nsize ? D : nsize);
         PF2.Reset(D<nsize ? D : nsize);

            // debias using the first point
         x0 = double(dc[0]);
         d10 = d1[0];
         d20 = d2[0];

            // use all the data in the sliding window
         for(i=0; i<nsize; i++) {
               // x is nominal receive time in units of count (DataInterval)
            x = double(dc[i]);
               // find the same count in the station buffers
            j = index(statn.CountBuffer,dc[i]);
            //if(j == -1) ?? TD
            x -= statn.RxTimeOffset[j]/CI.DataInterval;
            PF1.Add(d1[i]-d10,x-x0);
            PF2.Add(d2[i]-d20,x-x0);
         }

         change = false;

         //if(CI.Debug)
         //   for(i=0; i<nsize; i++) {
         //      x = double(dc[i]);         // count
         //      j = index(statn.CountBuffer,dc[i]);
         //      x -= statn.RxTimeOffset[j]/CI.DataInterval;
         //      //PF1.Add(d1[i]-d10,x-x0);
         //      //PF2.Add(d2[i]-d20,x-x0);
         //      oflog << "FIT " << site << " " << sat
         //         << " " << nc << " " << rawdat.count[nc]
         //         << " " << (D<nsize?D:nsize) << " " << nsize
         //         << fixed << setprecision(6)
         //         << " " << nbeg+i << " " << dc[i] << " " << rawdat.count[nbeg+i]
         //         << " " << x-x0 << " " << d1[i]-d10
         //         << " " << PF1.Evaluate(x-x0)
         //         << " " << d1[i]-d10 - PF1.Evaluate(x-x0)
         //         << endl;
         //}
      }

         // -------------------------------------------------------------
         // Process each point in the window/buffer
         // correct L1,L2,P1,P2 using the polynomials and dt=RxTTOffset+clk/c
         // statn.ClockBuffer contains clock solution
         // statn.RxTimeOffset contains SolutionEpoch - Rx timetag
         //
         // nominal time for point nc
      x = double(rawdat.count[nc]);
         // find the index of the same count in the station buffers
      j = index(statn.CountBuffer,rawdat.count[nc]);
         // time difference due to receiver clock, in units of count
      dx =  statn.RxTimeOffset[j]/CI.DataInterval
         + (statn.ClockBuffer[j]/C_GPS_M)/CI.DataInterval;
         // change in phase between nominal and true time
      dph1 = PF1.Evaluate(x-x0) - PF1.Evaluate(x-dx-x0);
      rawdat.L1[nc] += dph1;
      rawdat.P1[nc] += dph1 * wl1;
      dph2 = PF2.Evaluate(x-x0) - PF2.Evaluate(x-dx-x0);
      rawdat.L2[nc] += dph2;
      rawdat.P2[nc] += dph2 * wl2;

      //if(CI.Debug) oflog << "FIT " << site << " " << sat
      //   << " " << nc << " " << rawdat.count[nc]
      //   << fixed << setprecision(6)
      //   << " " << x-x0 << " " << dx
      //   << " " << statn.RxTimeOffset[nc]
      //   << " " << statn.ClockBuffer[nc]/C_GPS_M
      //   << " " << dph1
      //   << " " << dph2 << " eval" << endl;

         // -------------------------------------------------------------
         // remove old point(s) from the deques
      while(   (nend < len-1)      // a new end point would not go beyond buffer
            && (ngap < CI.MaxGap)  // & there would not be a big gap
            && (nend-nbeg+1 > N-1) // & window is full
            && (nc >= nbeg+nhalf)  // & current point is at mid-window or later
            ) {
         dc.pop_front();      // keep the deques parallel
         d1.pop_front();
         d2.pop_front();
         nbeg++;
         change = true;
      };

   }  // loop over counts

}
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 RecomputeFromEphemeris(void)
{
try {
   if(CI.Verbose) oflog << "BEGIN RecomputeFromEphemeris()" << endl;

   int nc;
   double angle,pwu,shadow;
   DayTime tt;
   GSatID sat;
   Position SV;
   Position West,North,Rx2Tx;
   CorrectedEphemerisRange CER;  // TD PreciseRange?
   map<string,Station>::iterator it;
   map<GSatID,RawData>::iterator jt;

      // loop over stations
   for(it=Stations.begin(); it != Stations.end(); it++) {
      //string label = it->first;
      Station& statn=it->second;

      //if(CI.Verbose) oflog << " Station " << it->first
      //   << " with " << statn.RawDataBuffers.size() << " raw buffers." << endl;

         // compute W and N unit vectors at this station,
         // rotated by antenna azimuth angle
      angle = statn.ant_azimuth * DEG_TO_RAD;
      if(fabs(angle) > 0.0001) {    // also below..
         Matrix<double> Rot;
         Rot = SingleAxisRotation(angle,1) * UpEastNorth(statn.pos);
         West = Position(-Rot(1,0),-Rot(1,1),-Rot(1,2));
         North = Position(Rot(2,0),Rot(2,1),Rot(2,2));
      }

         // loop over satellites
      for(jt=statn.RawDataBuffers.begin(); jt!=statn.RawDataBuffers.end(); jt++) {
         sat = jt->first;
         RawData& rawdat=jt->second;

         //if(CI.Verbose) oflog << "   Satellite " << sat
         //   << " with buffer size " << rawdat.count.size() << endl;

         if(rawdat.count.size() == 0) continue;

            // Loop over count (epochs). At each count, recompute the ephemeris
            // range and correct the phase for phase windup.
         for(nc=0; nc<rawdat.count.size(); nc++) {

               // nominal time is now the actual receive time of the data
            tt = FirstEpoch + rawdat.count[nc] * CI.DataInterval;

               // try to get the ephemeris info
            try {
                  // update ephemeris range and elevation
               rawdat.ER[nc] =
                  CER.ComputeAtReceiveTime(tt, statn.pos, sat, *pEph);
               rawdat.elev[nc] = CER.elevation;
               rawdat.az[nc] = CER.azimuth;

                  // correct for phase windup
               if(fabs(angle) > 0.0001) {    // also above..
                     // get the receiver-to-transmitter unit vector
                     // and the satellite position
                  Rx2Tx = Position(CER.cosines);
                  SV = Position(CER.svPosVel.x[0],
                                CER.svPosVel.x[1],
                                CER.svPosVel.x[2]);

                     // compute phase windup
                  pwu = PhaseWindup(tt,SV,Rx2Tx,West,North,shadow);

                  // TD eclipse alert
                  //if(shadow > 0.0) { ... }

                     // correct the phase
                  rawdat.L1[nc] += pwu * wl1;
                  rawdat.L2[nc] += pwu * wl2;
               }
            }
            catch(EphemerisStore::NoEphemerisFound& e) {
               // these should have been caught and removed before...
               oflog << "Warning - No ephemeris found for sat " << sat
                     << " at time "
                     << tt.printf("%Y/%02m/%02d %2H:%02M:%6.3f=%F/%10.3g")
                     << " in RecomputeFromEphemeris()" << endl;
               rawdat.ER[nc] = 0.0;
               rawdat.elev[nc] = -90.0;
               rawdat.az[nc] = 0.0;
            }
         }  // end loop over counts
      }  // loop over sats

   }  // loop over stations

   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); }
}

//------------------------------------------------------------------------------------
//------------------------------------------------------------------------------------