svcalc.c

GPS导航定位程序

    sok = sin((double)ok);
    cok = cos((double)ok);

    /* Transform position in the orbital plane to position in space. */

    *x = (double)(xpk*cok - ypk*cik*sok);
    *y = (double)(xpk*sok + ypk*cik*cok);
    *z = (double)(ypk*sik);

    *vx = (double)(xdot*cok - ydot*cik*sok + WGS84oe*(*y));
    *vy = (double)(xdot*sok + ydot*cik*cok - WGS84oe*(*x));
    *vz = (double)(ydot*sik);
}

/****************************************************************************    
* Function: void EphemerisPrediction(int sv, int wk, double sec,
*                                    double *svirel
*                                    double *x, double *y, double *z,
*                                    double *vx, double *vy, double *vz)
*
* Predictions a satellite position and velocity vector from the ephemeris.
*
* Input: sv - the satellite PRN number.
*        wk - the GPS week number.
*        sec - second in the GPS week.
*
* Output: svirel - relativistic correction term, s.
*         x - the satellite x co-ordinate, m.
*         y - the satellite y co-ordinate, m.
*         z - the satellite z co-ordinate, m.
*         vx - the satellite x co-ordinate velocity, m/s.
*         vy- the satellite y co-ordinate velocity, m/s.
*         vz - the satellite z co-ordinate velocity, m/s.
*
* Return Value: None.
****************************************************************************/
void EphemerisPrediction(int sv, double sec, double *svirel,
			 double *x, double *y,double *z,
			 double *vx, double *vy, double *vz)
{
    int iter;

    double dtemp,tk,M,E,cE,sE,dEdM,P,U,R,I,cU,sU,Xp,Yp,L,sI,cI,sL,cL,
	   ecc,s2P,c2P;
    double Edot,Pdot,Udot,Rdot,sUdot,cUdot,Xpdot,Ypdot,Idot,Ldot;
    double Mdot,sqrt1mee;

    ephstruc Ephem;

    PROTECT++;
    Ephem = ephs[sv-1];         /* Get the ephemeris for the SV concerned. */
    PROTECT--;

    /* Compute the difference, in seconds, between the epoch and wk & sec. */

    tk = sec - Ephem.toe;

    if(tk>SECONDS_IN_WEEK/2.0)
	tk -= SECONDS_IN_WEEK;
    else if(tk<(-SECONDS_IN_WEEK/2.0))
	tk += SECONDS_IN_WEEK;

    /* Mean anomaly, M (rads). */

    Mdot = Ephem.n0 + Ephem.deltan;
    M = Ephem.m0 + Mdot*tk;

    /* Obtain eccentric anomaly E by solving Kepler's equation. */

    ecc = Ephem.ecc;
    sqrt1mee = sqrt(1.0-ecc*ecc);
    E = M;
    for(iter=0;iter<20;iter++)
    {
	sE = sin(E);
	cE = cos(E);
	dEdM = 1.0 / (1.0 - ecc*cE);
	if(fabs(dtemp=(M-E+ecc*sE)*dEdM) < 1.0E-14)
	    break;
	E += dtemp;
    }

    /* Compute the relativistic correction term (seconds). */

    *svirel = -4.442809305E-10 * ecc * Ephem.sqrta * sE;

    Edot = dEdM * Mdot;

    /* Compute the argument of latitude, P. */

    P = atan2(sqrt1mee*sE,cE-ecc) + Ephem.olc;
    Pdot = sqrt1mee * dEdM * Edot;

    /* Generate harmonic correction terms for P and R. */

    s2P = sin(2.0*P);
    c2P = cos(2.0*P);

    /* Compute the corrected argument of latitude, U. */

    U = P + (dtemp=Ephem.cus*s2P+Ephem.cuc*c2P);
    sU = sin(U);
    cU = cos(U);
    Udot = Pdot * (1.0 + 2.0*(Ephem.cus*c2P - Ephem.cuc*s2P));
    sUdot = cU*Udot;
    cUdot = -sU*Udot;

    /* Compute the corrected radius, R. */

    R = Ephem.a*(1.0-ecc*cE) + (dtemp=Ephem.crs*s2P+Ephem.crc*c2P);
    Rdot = Ephem.a*ecc*sE*Edot + 2.0*Pdot*(Ephem.crs*c2P - Ephem.crc*s2P);

    /* Compute the corrected orbital inclination, I. */

    I = Ephem.in0 + Ephem.idot*tk + (dtemp=Ephem.cis*s2P+Ephem.cic*c2P);
    sI = sin(I);
    cI = cos(I);
    Idot = Ephem.idot + 2.0*Pdot*(Ephem.cis*c2P - Ephem.cic*s2P);

    /* Compute the satellite's position in its orbital plane, (Xp,Yp). */

    Xp = R*cU;
    Yp = R*sU;
    Xpdot = Rdot*cU + R*cUdot;
    Ypdot = Rdot*sU + R*sUdot;

    /* Compute the longitude of the ascending node, L. */

    L = Ephem.om0 + tk*(Ephem.omd-WGS84oe) - WGS84oe*Ephem.toe;
    Ldot = Ephem.omd - WGS84oe;
    sL = sin(L);
    cL = cos(L);

    /* Compute the satellite's position in space, (x,y,z). */

    *x = Xp*cL - Yp*cI*sL;
    *y = Xp*sL + Yp*cI*cL;
    *z = Yp*sI;

    /* Satellite's velocity, (vx,vy,vz). */

    *vx = - Ldot*(*y)
	  + Xpdot*cL
	  - Ypdot*cI*sL
	  + Yp*sI*Idot*sL;

    *vy = Ldot*(*x)
	  + Xpdot*sL
	  + Ypdot*cI*cL
	  - Yp*sI*Idot*cL;

    *vz = + Yp*cI*Idot
	  + Ypdot*sI;
}

/****************************************************************************    
* Function: void AzimuthElevationAndDoppler(int calcsv)
*
* Calculate the azimuth, elevation and Doppler for a satellite.
*
* Input: calcsv - the satellite PRN number.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void AzimuthElevationAndDoppler(int calcsv)
{
    int gwk;
    unsigned long ctic;
    double gsec;
    float elevation,azimuth,doppler;

    navstatestruc N;

    if(alms[calcsv].vflg!=VALID && ephs[calcsv].vflg!=VALID)
	return;

    CurrentTIC(&ctic);
    TICToGpsTime(ctic,&gwk,&gsec);

    /* Obtain a local copy of the current navigation state. */

    PROTECT++;
    N = CurNavState;
    PROTECT--;


    /* Update elevation, azimuth and Doppler predictions for this SV. */

    SVPredicts(calcsv+1,gwk,gsec,N.topo,N.x,N.y,N.z,&elevation,
	       &azimuth,&doppler,&sverc[calcsv],
	       &sviono[calcsv],&svtropo[calcsv]);
    ielvd[calcsv] = elevation * R2D;
    iazid[calcsv] = azimuth * R2D;

    /* Convert to range of 0 to 360 degrees. */

    if(iazid[calcsv] < 0)
	iazid[calcsv] = iazid[calcsv] + 360;
    idopp[calcsv] = doppler + 0.5;
}

/****************************************************************************    
* Function: void ElevationList(int highest[])
*
* Produces a satellite list ordered by elevation.
*
* Input: None.
*
* Output: highest - SV numbers order by elevation (highest first).
*
* Return Value: None.
****************************************************************************/
void ElevationList(int highest[])
{
    int i,isat,highelv[MAXCHANNELS],itemp;

    /* Select the highest SV's. */

    for(i=0;i<ActiveChannels;i++)
    {
	highest[i] = 0;
	highelv[i] = -99;                    /* Initial highest elevation. */
    }

    for(isat=1;isat<33;isat++)
    {
	if((alms[isat-1].vflg!=1) && (ephs[isat-1].vflg==0))
	    continue;
	if(Deselect[isat-1])
	    continue;
	if( ((ephs[isat-1].vflg==1) && (ephs[isat-1].s1hlth))
	    || ((alms[isat-1].vflg==1) && (alms[isat-1].almhlth)) )
	    continue;

	if((ielvd[isat-1]) > highelv[ActiveChannels-1])
	{
	    highest[ActiveChannels-1] = isat;
	    highelv[ActiveChannels-1] = ielvd[isat-1];
	    for(i=ActiveChannels-2; i>=0; --i)
	    {
		if (highelv[i+1] <= highelv[i])
		    break;
		itemp = highest[i+1];
		highest[i+1] = highest[i];
		highest[i] = itemp;
		itemp = highelv[i+1];
		highelv[i+1] = highelv[i];
		highelv[i] = itemp;
	    }
	}
    }
}

/****************************************************************************    
* Function: void PredictAll(void)
*
* Predicts Doppler, elevation, azimuth, and deterministic corrections for
* all satellites having ephemeris or almanac data.
*
* Input: None.
*
* Output: None.
*
* Return Value: None.
****************************************************************************/
void PredictAll(void)
{
    int isat,gwk;

    unsigned long ctic;

    float elevation,azimuth,doppler;

    double gsec;

    navstatestruc N;

    PROTECT++;
    N = CurNavState;
    PROTECT--;

    CurrentTIC(&ctic);
    TICToGpsTime(ctic,&gwk,&gsec);
    for(isat=1;isat<MAXSATELLITES;isat++)
    {
	if(alms[isat-1].vflg!=1 && ephs[isat-1].vflg!=1)
	    continue;
	PROTECT++;
	SVPredicts(isat,gwk,gsec,N.topo,N.x,N.y,N.z,&elevation,&azimuth,
		   &doppler,&sverc[isat-1],&sviono[isat-1],&svtropo[isat-1]);
	ielvd[isat-1] = elevation * R2D;
	iazid[isat-1] = azimuth * R2D;
	if(iazid[isat-1] < 0)
	    iazid[isat-1] = iazid[isat-1] + 360;               /* degrees. */
	idopp[isat-1] = doppler + 0.5;
	PROTECT--;
    }
}