/* sbas ephemeris to satellite position and clock bias -------------------------
* compute satellite position and clock bias with sbas ephemeris
* args : gtime_t time I time (gpst)
* seph_t *seph I sbas ephemeris
* double *rs O satellite position {x,y,z} (ecef) (m)
* double *dts O satellite clock bias (s)
* double *var O satellite position and clock variance (m^2)
* return : none
* notes : see ref [3]
*-----------------------------------------------------------------------------*/
extern void seph2pos(gtime_t time, const seph_t *seph, double *rs, double *dts,
double *var)
{
double t;
int i;
trace(4,"seph2pos: time=%s sat=%2d\n",time_str(time,3),seph->sat);
t=timediff(time,seph->t0);
for (i=0;i<3;i++) {
rs[i]=seph->pos[i]+seph->vel[i]*t+seph->acc[i]*t*t/2.0;
}
*dts=seph->af0+seph->af1*t;
*var=var_uraeph(seph->sva);
}
/* -- void eph2pos(gtime_t time, const eph_t *eph,double *rs,double *dts,double *var)--------------------------------------
* Description : broadcast ephemeris to sat position and clock bias
* Parameters :
* time I transmission time Tsv
* rs O sat positions and velocities
* dts O sat clock correction (bias, drift)
* var O sat positions and clock error variance
* Return : 0 no ephemeris
*/
void eph2pos(gtime_t time, const eph_t *eph,double *rs,double *dts,double *var)
{
double Mk,tk,Ek,E,sinEk,cosEk,phik,sin2p,cos2p;
double uk,rk,ik,e=eph->e,xk,yk;
tk=timediff(time,eph->toe);
Mk=eph->M0+(sqrt(MUY/(eph->A*eph->A*eph->A))+eph->deln)*tk;
for (Ek=Mk,E=0.0;fabs(Ek-E)>1e-14;)//newton method
{
E=Ek;
Ek-=(Ek-e*sin(Ek)-Mk)/(1.0-e*cos(Ek));
}
//start=TIM2->CNT;
sinEk=sin(Ek);
//SendIntStr(TIM2->CNT-start);
cosEk=cos(Ek);
phik=atan2(sqrt(1-e*e)*sinEk,cosEk-e)+eph->omg;
sin2p=sin(2.0*phik);
cos2p=cos(2.0*phik);
uk=phik+eph->cus*sin2p+eph->cuc*cos2p;
rk=eph->A*(1-e*cosEk)+eph->crs*sin2p+eph->crc*cos2p;
ik=eph->i0+eph->cis*sin2p+eph->cic*cos2p+eph->idot*tk;
xk=rk*cos(uk);
yk=rk*sin(uk);
phik=eph->OMG0+(eph->OMgd-OMGE)*tk-OMGE*eph->toes;
sin2p=sin(phik);
cos2p=cos(phik);
cosEk=cos(ik);
rs[0]=xk*cos2p-yk*cosEk*sin2p;
rs[1]=xk*sin2p+yk*cosEk*cos2p;
rs[2]=yk*sin(ik);
tk=timediff(time,eph->toc);
(*dts)=(eph->f0)+(eph->f1)*tk+(eph->f2)*tk*tk;
(*dts)+=F*e*sqrt(eph->A)*sinEk;//hieu ung tuong doi tinh
*var=var_uraeph(eph->sva);
}
/* broadcast ephemeris to satellite position and clock bias --------------------
* compute satellite position and clock bias with broadcast ephemeris (gps,
* galileo, qzss)
* args : gtime_t time I time (gpst)
* eph_t *eph I broadcast ephemeris
* double *rs O satellite position (ecef) {x,y,z} (m)
* double *dts O satellite clock bias (s)
* double *var O satellite position and clock variance (m^2)
* return : none
* notes : see ref [1],[7],[8]
* satellite clock includes relativity correction without code bias
* (tgd or bgd)
*-----------------------------------------------------------------------------*/
extern void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
double *var)
{
double tk,M,E,Ek,sinE,cosE,u,r,i,O,sin2u,cos2u,x,y,sinO,cosO,cosi,mu,omge;
double xg,yg,zg,sino,coso;
int n,sys,prn;
trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat);
if (eph->A<=0.0) {
rs[0]=rs[1]=rs[2]=*dts=*var=0.0;
return;
}
tk=timediff(time,eph->toe);
switch ((sys=satsys(eph->sat,&prn))) {
case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break;
case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break;
default: mu=MU_GPS; omge=OMGE; break;
}
M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk;
for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&n<MAX_ITER_KEPLER;n++) {
Ek=E; E-=(E-eph->e*sin(E)-M)/(1.0-eph->e*cos(E));
}
if (n>=MAX_ITER_KEPLER) {
trace(2,"kepler iteration overflow sat=%2d\n",eph->sat);
return;
}
sinE=sin(E); cosE=cos(E);
trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek);
u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg;
r=eph->A*(1.0-eph->e*cosE);
i=eph->i0+eph->idot*tk;
sin2u=sin(2.0*u); cos2u=cos(2.0*u);
u+=eph->cus*sin2u+eph->cuc*cos2u;
r+=eph->crs*sin2u+eph->crc*cos2u;
i+=eph->cis*sin2u+eph->cic*cos2u;
x=r*cos(u); y=r*sin(u); cosi=cos(i);
/* beidou geo satellite (ref [9]) */
if (sys==SYS_CMP&&prn<=5) {
O=eph->OMG0+eph->OMGd*tk-omge*eph->toes;
sinO=sin(O); cosO=cos(O);
xg=x*cosO-y*cosi*sinO;
yg=x*sinO+y*cosi*cosO;
zg=y*sin(i);
sino=sin(omge*tk); coso=cos(omge*tk);
rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5;
rs[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5;
rs[2]=-yg*SIN_5+zg*COS_5;
}
else {
O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes;
sinO=sin(O); cosO=cos(O);
rs[0]=x*cosO-y*cosi*sinO;
rs[1]=x*sinO+y*cosi*cosO;
rs[2]=y*sin(i);
}
tk=timediff(time,eph->toc);
*dts=eph->f0+eph->f1*tk+eph->f2*tk*tk;
/* relativity correction */
*dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT);
/* position and clock error variance */
*var=var_uraeph(eph->sva);
}
