/* ephxyz.cpp */
/* xyz and relativistic correction from ephemeris data */
/* ************************************ */
/* Peter H. Dana */
/* POB 1297 */
/* Georgetown, TX 78627 */
/* ************************************ */
#include "gsinclud.h"
int ephxyz(ephemeris,gpstime,svs)
struct ephemeris_struct *ephemeris; /* pointer to ephemeris */
struct gpstime_struct *gpstime; /* pointer to gps time */
struct sv_struct *svs; /* pointer to svs */
{
/* ecef xyz from icd-gps-200 */
/* !!!!!!!!!!!!!!!!!!!WARNING!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
/* ! this software assumes that all semicircle parameters ! */
/* ! have been converted to radians. ! */
/* !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!! */
double n0; /* computed mean motion */
double n; /* corrected mean motion */
double tk; /* time from ephemeris reference epoch */
double mk; /* mean anomaly */
double ek; /* eccentric anomaly */
double vk; /* true anomaly */
double pk; /* argument of latitude */
double sin2pk; /* sin 2*pk */
double cos2pk; /* cos 2*pk */
double uk; /* corrected agument of latitude */
double duk; /* latitude correction */
double drk; /* radius correction */
double dik; /* inclination correction */
double rk; /* corrected radius */
double ik; /* corrected inclination */
double xkp; /* x position in orbital plane */
double ykp; /* y position in orbital plane */
double ok; /* corrected longitude of ascending node */
double sinok; /* sin of ok */
double cosok; /* cos of ok */
double ykpcosik; /* ykp * cos (ik) */
double sinvk; /* sin (vk) */
double cosvk; /* cos (vk) */
double sinek; /* sin (ek) */
double cosek; /* cos (ek) */
/* *********** */
svs->id=ephemeris->id;
n0=sqrt(MU/pow(ephemeris->sqrta,6.0)); /* computed mean motion */
tk=gpstime->seconds-ephemeris->toe; /* time from ephemeris epoch */
if (tk>HALFWEEK) tk-=WEEK;
if(tk<-HALFWEEK) tk+=WEEK;
n=n0+ephemeris->dn; /* corrected mean motion */
mk=ephemeris->ma+n*tk; /* mean anomaly */
kepler(&mk,&ephemeris->ecc,&ek); /* kepler's equation for eccentric anomaly */
cosek=cos(ek);
sinek=sin(ek);
cosvk=(cosek-ephemeris->ecc);
sinvk=sqrt(1.0-ephemeris->ecc*ephemeris->ecc)*sinek;
vk=atan2(sinvk,cosvk); /* true anomaly */
if (vk<0.0) vk+=2*PI;
pk=vk+ephemeris->aop; /* argument of latitude */
sin2pk=sin(2.0*pk);
cos2pk=cos(2.0*pk);
duk=ephemeris->cus*sin2pk+ephemeris->cuc*cos2pk; /* argument of latitude correction */
drk=ephemeris->crc*cos2pk+ephemeris->crs*sin2pk; /* radius correction */
dik=ephemeris->cic*cos2pk+ephemeris->cis*sin2pk; /* correction to inclination */
uk=pk+duk; /* latitude */
rk=ephemeris->sqrta*ephemeris->sqrta*(1.0-ephemeris->ecc*cosek)+drk; /* corrcted radius */
ik=ephemeris->oinc+dik+ephemeris->idot*tk; /* corrected inclination */
xkp=rk*cos(uk); /* x in orbital plane */
ykp=rk*sin(uk); /* y in orbital plane */
ok=ephemeris->omega0+(ephemeris->omegadot-OMEGA_DOT_E)*tk-OMEGA_DOT_E*
ephemeris->toe; /* longitude of ascending node */
ykpcosik=ykp*cos(ik);
sinok=sin(ok);
cosok=cos(ok);
/* sv xyz */
svs->xyz.x=xkp*cosok-ykpcosik*sinok;
svs->xyz.y=xkp*sinok+ykpcosik*cosok;
svs->xyz.z=ykp*sin(ik);
/* sv relativistic correction in meters */
svs->rel=C*F*ephemeris->sqrta*ephemeris->ecc*sinek;
/* set time tag */
svs->iode=ephemeris->iode2;
return(1);
} /* module end */
/* kepler.cpp */
#include "gsinclud.h"
kepler(mk,ecc,ek)
double *mk;
double *ecc;
double *ek;
/* keplers eccentric anomaly */
{
/* iteration with wegstein's accelerator */
double x;
double y;
double x1,y1,x2;
int i;
x=*mk;
y=*mk-(x-*ecc*sin(x));
x1=x;
x=y;
for(i=0;i<16;i++){
x2=x1;
x1=x;
y1=y;
y=*mk-(x-*ecc*sin(x));
if(fabs(y-y1)<1.0E-15) break;
x=(x2*y-x*y1)/(y-y1);
} /* for iterations */
*ek=x;
}/* program done */
/* ephemeris structure */
struct ephemeris_struct {
int id; /* sv id PRN code */
double ma; /* mean anomaly radians */
double dn; /* mean motion difference */
double ecc; /* eccentricity */
double sqrta; /* square root of semi-major axis */
double omega0; /* longitude of ascending node */
double oinc; /* oribtal inclination */
double aop; /* angle of perigee */
double omegadot; /* rate of right ascension */
double idot; /* rate of inclination angle */
double cuc; /* cosine correction to latitude */
double cus; /* sine correction to latitude */
double crc; /* cosine correction to radius */
double crs; /* sine correction to radius */
double cic; /* cosine correction to inclination */
double cis; /* sine correction to inclination */
long toe; /* reference time ephemeris */
long iode2; /* subframe 2 issue of data ephemeris */
long iode3; /* subframe 3 issue of data ephemeris */
};