intrpsp3.cpp

这是用于生成GPS星空图的一个c++程序

   {
      cerr << "Error reading the PRNs from the header for SP3 file: "
      << endl << pathFilename << endl
      << "Number of SVs expected: " << setw(6) << numberSP3svs << endl
      << "Number of PRNs read in: " << setw(6) << numberGoodPRNs << endl;
      return(1);
   }
   else
     return(0);


} // end of method SP3File::readHeader()

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


int SP3File::getSVPosVel(DateTime tuser, unsigned short PRNid, double rvec[])
{
   double    p, q, p2, q2, sum1, sum2, trun;
   int       i, j, k, l, m, n, jsv;
   long      jmax, jmid, jmin, mbeg, mend, muse;
   bool      foundSV = false;
   char      inputRecC[ 256 ];
   string    inputRec;
   string    temp;

   int  iret;
//   fstream   dout;
//   dout.open("hilla.out",ios_base::out);
//   if( !dout ) cerr << "Cannot open hilla.out file !!! " << endl;


   for( i = 0; i < (4 + numberSVparams); i++)
     rvec[i] = 0.0;

//   dout << "numberSP3svs = " << numberSP3svs << endl;
//   dout << "numberSVparams = " << numberSVparams << endl;
//   dout << "numberSP3epochs = " << numberSP3epochs << endl;
//   dout << "lastEpochRead = " << lastEpochRead << endl;

   for( i = 1; i <= numberSP3svs; i++)
   {
//     dout << "i,prn: " << i << " " << sp3PRNs[i] << endl;
     if( PRNid == sp3PRNs[i] )
     {
       jsv = i;
       foundSV = true;
       break;  // exit loop over SP3svs once a match is found
     }
   }


   if( !foundSV )
   {
     //cerr << "Cannot find PRN " << PRNid << " in SP3 file "
     //<< pathFilename << endl << "Terminate Program." << endl;
     return( -2 );
   }


   trun = (tuser - SP3StartTime)*86400.0;  // diff between DateTimes = days
   trun = trun/SP3interval + 1.0;
//   dout << "trun as epoch span = " << trun << endl;
   jmin = (long)trun - 4;
   jmax = (long)trun + 5;
//   dout << "jmin, jmax: " << jmin << " " << jmax << endl;

   if( jmin < 1 )
   {
       jmin = 1;
       jmax = jmin + 9;
       if( jmax > (long) numberSP3epochs)
       {
         return( -3 );  // not enough data at beginning of SP3 file
       }
   }

   if( jmax > (long) numberSP3epochs )
   {
       jmax = numberSP3epochs;
       jmin = jmax - 9;
       if( jmin < 1 )
       {
	   return( -4 );  // not enough data at end of SP3 file
       }
   }


// move in file
   if( jmax != (long) lastEpochRead   &&
       fabs( (double)lastEpochRead - (trun + 5.0) ) > (1.0/SP3interval) )
   {
	 // move in file backward
	 if( jmax < (long) lastEpochRead )
	 {
	     iret = readHeader();
	     if( iret != 0 ) return( iret );
         }

	 // move in file forward
    if( lastEpochRead > 0 )
    {
         for( k = 1; k <= numberSP3svs; k++ )
	 {
	   for( j = 1; j <= numberSVparams; j++ )
	   {
	     for( i = 1; i <= (int) lastEpochRead - jmin + 1; i++ )
	     {
 		 m = i + 10 - (lastEpochRead - jmin + 1);
//                 dout << "file forward ====  m = " << m << endl;
		 inputValues[i][j][k] = inputValues[m][j][k];
//                 dout << " i,j,k,yye: " << i << " " << j << " " << k <<
//                  " " << setw(30) << setprecision(15) << inputValues[i][j][k];
             }
	   }
	 }
     } // do this section only if lastEpochRead != 0


         // skip over lines to get to the correct epoch in the SP3 file
         for( i = lastEpochRead + 1; i <= jmin - 1; i++ )
	 {
	   for( j = 1; j <= numberSP3svs + 1; j++ )
	   {
             if( fileStream.getline(inputRecC, 255, '\n') == NULL )
	     {
		 cerr << "Error skipping lines in the SP3 file." << endl;
		 return( -1 );
             }
//             dout << " aline = " << inputRecC << endl;
	   }
	 }

	 // read the data into the input values
	 if( (long) (lastEpochRead + 1) > jmin)
	   m = lastEpochRead + 1;
	 else
	   m = jmin;

         for( i = m; i <= jmax; i++ )
	 {
	   l = i - jmin + 1;
           if( fileStream.getline(inputRecC, 255, '\n') == NULL )
	   {
	      cerr << "Error skipping time tag line in the SP3 file." << endl;
	      return( -1 );
           }
	   for( k = 1; k <= numberSP3svs; k++ )
	   {
              if( fileStream.getline(inputRecC, 255, '\n') == NULL )
              {
               cerr << "Error reading input values in the SP3 file." << endl;
	       return( -1 );
              }
	      inputRec = inputRecC;
	      for( j = 1; j <= numberSVparams; j++ )
	      {
		  temp = inputRec.substr( (4 + ((j - 1)*14)), 14);
		  inputValues[l][j][k] = atof( temp.c_str() );
                  if( inputValues[l][j][k] > 999999.9 )
                     inputValues[l][j][k] = 0.0;   // do for bad clocks
// dout << "read new yye (i,j,k,yye): " << setw(8) << (i - jmin + 1) << " "
// << j << " " << k << " " << setw(30) << setprecision(15) <<
// inputValues[l][j][k] << endl;
	      }

	   }
	 }
	 lastEpochRead = jmax;
//         dout << "new jlaste: " << setw(8) << lastEpochRead << endl;


	 // interpolate terms for position and velocity
	 for( i = 1; i <= numberSP3svs; i++ )
	 {
	   n = 4;
	   if( numberSVparams < 4 ) n = numberSVparams;
	   for( j = 1; j <= n; j++ )
	   {
	     for( k = 1; k <= 2; k++ )
	     {
                jmid = 4 + k;
		for( l = 1; l <= 5; l++ )
		{
                    intrpCoeff[l][k][j][i] = 0.0;
		    for( m = 5; m >= 2; m-- )
		    {
		       intrpCoeff[l][k][j][i] = intrpCoeff[l][k][j][i] +
		       EVCF[m - 1][l - 1] *
		       (inputValues[jmid + m - 1][j][i]
		            + inputValues[jmid - m + 1][j][i]);
// dout << " 1st iloop l,k,j,i,m: " << l << " " << k << " " << j << " " << i
//      << " " << m << endl;
                    }
		    intrpCoeff[l][k][j][i] = intrpCoeff[l][k][j][i] +
		    EVCF[1 - 1][l - 1] * inputValues[jmid][j][i];

		    intrpCoeff[l][k][j + numberSVparams][i] =
		      intrpCoeff[l][k][j][i] * MFACT[l - 1]/SP3interval;
                }
             }

           }
	   for( j = 5; j <= numberSVparams; j++ )
	   {
	     for( k = 1; k <= 2; k++ )
	     {
                jmid = 4 + k;
		for( l = 1; l <= 5; l++ )
		{
                    intrpCoeff[l][k][j][i] = 0.0;
		    for( m = 5; m >= 2; m-- )
		    {
		       intrpCoeff[l][k][j][i] = intrpCoeff[l][k][j][i] +
		       EVCF[m - 1][l - 1] *
		       (inputValues[jmid + m - 1][j][i]
		            + inputValues[jmid - m + 1][j][i]);
                    }
		    intrpCoeff[l][k][j][i] = intrpCoeff[l][k][j][i] +
		    EVCF[1 - 1][l - 1] * inputValues[jmid][j][i];
                }
             }

           }


       } // end of loop over satellites


     } // end of if test for jmax != lastEpochRead && ...

     // initialize interpolation parameters
     p = trun - lastEpochRead + 5;
     q = 1.0 - p;
     p2 = p * p;
     q2 = q * q;

//     dout << "p  = " << p  << endl;
//     dout << "q  = " << q  << endl;
//     dout << "p2 = " << p2 << endl;
//     dout << "q2 = " << q2 << endl;

     mbeg = 1;
     mend = numberSVparams + 4;   // do all: x,y,z,clk, xdot,ydot,zdot,clk-dot
//     dout << "mbeg,mend: " << mbeg << " " << mend << endl;

     // interpolate
     for( m = mbeg; m <= mend; m++ )
     {
        if( m <= 4 )
	  muse = m;
	else if( m > 4  &&  m <= numberSVparams )
	  muse = m + 4;

        if( m > numberSVparams  &&  m <= (numberSVparams + 4) )
	  muse = m - numberSVparams + 4;


        sum1 = 0.0;
	sum2 = 0.0;
	for( j = 5; j >= 2; j-- )
	{
//           dout << "3rd iloop j,m,jsv,muse: " << j << " " << m << " " <<
//           " " << jsv << " " << muse << endl;
	   sum1 = p2*(sum1 + intrpCoeff[j][2][m][jsv]);
	   sum2 = q2*(sum2 + intrpCoeff[j][1][m][jsv]);
   	}

	if( m <= numberSVparams )
	  rvec[muse - 1] = p * (intrpCoeff[1][2][m][jsv] + sum1 ) +
	    q * (intrpCoeff[1][1][m][jsv] + sum2 );
	else
	  rvec[muse - 1] = (intrpCoeff[1][2][m][jsv] + sum1 ) -
	    (intrpCoeff[1][1][m][jsv] + sum2 );

   }

//   dout << setw(30) << setprecision(15) << endl << endl;
//   dout << "inside rvec[0] = " << rvec[0] << endl;
//   dout << "inside rvec[1] = " << rvec[1] << endl;
//   dout << "inside rvec[2] = " << rvec[2] << endl;
//   dout << "inside rvec[3] = " << rvec[3] << endl;
//   dout << "inside rvec[4] = " << rvec[4] << endl;
//   dout << "inside rvec[5] = " << rvec[5] << endl;
//   dout << "inside rvec[6] = " << rvec[6] << endl;
//   dout << "inside rvec[7] = " << rvec[7] << endl;

//   dout.close();
   return(0);

} // end of method SP3File::getSVPosVel()