/*------------------------------------------------------------------------------ * tle.c: NORAD TLE (two line element) functions * * Copyright (C) 2012-2020 by T.TAKASU, All rights reserved. * * references: * [1] F.R.Hoots and R.L.Roehrich, Spacetrack report No.3, Models for * propagation of NORAD element sets, December 1980 * [2] D.A.Vallado, P.Crawford, R.Hujsak and T.S.Kelso, Revisiting * Spacetrack Report #3, AIAA 2006-6753, 2006 * [3] CelesTrak (http://www.celestrak.com) * * version : $Revision:$ $Date:$ * history : 2012/11/01 1.0 new * 2013/01/25 1.1 fix bug on binary search * 2014/08/26 1.2 fix bug on tle_pos() to get tle by satid or desig * 2020/11/30 1.3 fix problem on duplicated names in a satellite *-----------------------------------------------------------------------------*/ #include "rtklib.h" /* SGP4 model propagator by STR#3 (ref [1] sec.6,11) -------------------------*/ #define DE2RA 0.174532925E-1 #define E6A 1.E-6 #define PIO2 1.57079633 #define QO 120.0 #define SO 78.0 #define TOTHRD 0.66666667 #define TWOPI 6.2831853 #define X3PIO2 4.71238898 #define XJ2 1.082616E-3 #define XJ3 -0.253881E-5 #define XJ4 -1.65597E-6 #define XKE 0.743669161E-1 #define XKMPER 6378.135 #define XMNPDA 1440.0 #define AE 1.0 #define CK2 5.413080E-4 /* = 0.5*XJ2*AE*AE */ #define CK4 0.62098875E-6 /* = -0.375*XJ4*AE*AE*AE*AE */ #define QOMS2T 1.88027916E-9 /* = pow((QO-SO)*AE/XKMPER,4.0) */ #define S 1.01222928 /* = AE*(1.0+SO/XKMPER) */ static void SGP4_STR3(double tsince, const tled_t *data, double *rs) { double xnodeo,omegao,xmo,eo,xincl,xno,xndt2o,xndd6o,bstar; double a1,cosio,theta2,x3thm1,eosq,betao2,betao,del1,ao,delo,xnodp,aodp,s4; double qoms24,perige,pinvsq,tsi,eta,etasq,eeta,psisq,coef,coef1,c1,c2,c3,c4; double c5,sinio,a3ovk2,x1mth2,theta4,xmdot,x1m5th,omgdot,xhdot1,xnodot; double omgcof,xmcof,xnodcf,t2cof,xlcof,aycof,delmo,sinmo,x7thm1,c1sq,d2,d3; double d4,t3cof,t4cof,t5cof,xmdf,omgadf,xnoddf,omega,xmp,tsq,xnode,delomg; double delm,tcube,tfour,a,e,xl,beta,xn,axn,xll,aynl,xlt,ayn,capu,sinepw; double cosepw,epw,ecose,esine,elsq,pl,r,rdot,rfdot,betal,cosu,sinu,u,sin2u; double cos2u,rk,uk,xnodek,xinck,rdotk,rfdotk,sinuk,cosuk,sinik,cosik,sinnok; double cosnok,xmx,xmy,ux,uy,uz,vx,vy,vz,x,y,z,xdot,ydot,zdot; double temp,temp1,temp2,temp3,temp4,temp5,temp6,tempa,tempe,templ; int i,isimp; xnodeo=data->OMG*DE2RA; omegao=data->omg*DE2RA; xmo=data->M*DE2RA; xincl=data->inc*DE2RA; temp=TWOPI/XMNPDA/XMNPDA; xno=data->n*temp*XMNPDA; xndt2o=data->ndot*temp; xndd6o=data->nddot*temp/XMNPDA; bstar=data->bstar/AE; eo=data->ecc; /* * recover original mean motion (xnodp) and semimajor axis (aodp) * from input elements */ a1=pow(XKE/xno,TOTHRD); cosio=cos(xincl); theta2=cosio*cosio; x3thm1=3.0*theta2-1.0; eosq=eo*eo; betao2=1.0-eosq; betao=sqrt(betao2); del1=1.5*CK2*x3thm1/(a1*a1*betao*betao2); ao=a1*(1.0-del1*(0.5*TOTHRD+del1*(1.0+134.0/81.0*del1))); delo=1.5*CK2*x3thm1/(ao*ao*betao*betao2); xnodp=xno/(1.0+delo); aodp=ao/(1.0-delo); /* * initialization * for perigee less than 220 kilometers, the isimp flag is set and * the equations are truncated to linear variation in sqrt a and * quadratic variation in mean anomaly. also, the c3 term, the * delta omega term, and the delta m term are dropped. */ isimp=0; if ((aodp*(1.0-eo)/AE)<(220.0/XKMPER+AE)) isimp=1; /* for perigee below 156 km, the values of s and qoms2t are altered */ s4=S; qoms24=QOMS2T; perige=(aodp*(1.0-eo)-AE)*XKMPER; if (perige<156.0) { s4=perige-78.0; if (perige<=98.0) s4=20.0; qoms24=pow((120.0-s4)*AE/XKMPER,4.0); s4=s4/XKMPER+AE; } pinvsq=1.0/(aodp*aodp*betao2*betao2); tsi=1.0/(aodp-s4); eta=aodp*eo*tsi; etasq=eta*eta; eeta=eo*eta; psisq=fabs(1.0-etasq); coef=qoms24*pow(tsi,4.0); coef1=coef/pow(psisq,3.5); c2=coef1*xnodp*(aodp*(1.0+1.5*etasq+eeta*(4.0+etasq))+0.75* CK2*tsi/psisq*x3thm1*(8.0+3.0*etasq*(8.0+etasq))); c1=bstar*c2; sinio=sin(xincl); a3ovk2=-XJ3/CK2*pow(AE,3.0); c3=coef*tsi*a3ovk2*xnodp*AE*sinio/eo; x1mth2=1.0-theta2; c4=2.0*xnodp*coef1*aodp*betao2*(eta* (2.0+0.5*etasq)+eo*(0.5+2.0*etasq)-2.0*CK2*tsi/ (aodp*psisq)*(-3.0*x3thm1*(1.0-2.0*eeta+etasq* (1.5-0.5*eeta))+0.75*x1mth2*(2.0*etasq-eeta* (1.0+etasq))*cos(2.0*omegao))); c5=2.0*coef1*aodp*betao2*(1.0+2.75*(etasq+eeta)+eeta*etasq); theta4=theta2*theta2; temp1=3.0*CK2*pinvsq*xnodp; temp2=temp1*CK2*pinvsq; temp3=1.25*CK4*pinvsq*pinvsq*xnodp; xmdot=xnodp+0.5*temp1*betao*x3thm1+0.0625*temp2*betao* (13.0-78.0*theta2+137.0*theta4); x1m5th=1.0-5.0*theta2; omgdot=-0.5*temp1*x1m5th+0.0625*temp2*(7.0-114.0*theta2+ 395.0*theta4)+temp3*(3.0-36.0*theta2+49.0*theta4); xhdot1=-temp1*cosio; xnodot=xhdot1+(0.5*temp2*(4.0-19.0*theta2)+2.0*temp3*(3.0- 7.0*theta2))*cosio; omgcof=bstar*c3*cos(omegao); xmcof=-TOTHRD*coef*bstar*AE/eeta; xnodcf=3.5*betao2*xhdot1*c1; t2cof=1.5*c1; xlcof=0.125*a3ovk2*sinio*(3.0+5.0*cosio)/(1.0+cosio); aycof=0.25*a3ovk2*sinio; delmo=pow(1.0+eta*cos(xmo),3.0); sinmo=sin(xmo); x7thm1=7.0*theta2-1.0; if (isimp!=1) { c1sq=c1*c1; d2=4.0*aodp*tsi*c1sq; temp=d2*tsi*c1/3.0; d3=(17.0*aodp+s4)*temp; d4=0.5*temp*aodp*tsi*(221.0*aodp+31.0*s4)*c1; t3cof=d2+2.0*c1sq; t4cof=0.25*(3.0*d3+c1*(12.0*d2+10.0*c1sq)); t5cof=0.2*(3.0*d4+12.0*c1*d3+6.0*d2*d2+15.0*c1sq*(2.0*d2+c1sq)); } else { d2=d3=d4=t3cof=t4cof=t5cof=0.0; } /* update for secular gravity and atmospheric drag */ xmdf=xmo+xmdot*tsince; omgadf=omegao+omgdot*tsince; xnoddf=xnodeo+xnodot*tsince; omega=omgadf; xmp=xmdf; tsq=tsince*tsince; xnode=xnoddf+xnodcf*tsq; tempa=1.0-c1*tsince; tempe=bstar*c4*tsince; templ=t2cof*tsq; if (isimp==1) { delomg=omgcof*tsince; delm=xmcof*(pow(1.0+eta*cos(xmdf),3.0)-delmo); temp=delomg+delm; xmp=xmdf+temp; omega=omgadf-temp; tcube=tsq*tsince; tfour=tsince*tcube; tempa=tempa-d2*tsq-d3*tcube-d4*tfour; tempe=tempe+bstar*c5*(sin(xmp)-sinmo); templ=templ+t3cof*tcube+tfour*(t4cof+tsince*t5cof); } a=aodp*pow(tempa,2.0); e=eo-tempe; xl=xmp+omega+xnode+xnodp*templ; beta=sqrt(1.0-e*e); xn=XKE/pow(a,1.5); /* long period periodics */ axn=e*cos(omega); temp=1.0/(a*beta*beta); xll=temp*xlcof*axn; aynl=temp*aycof; xlt=xl+xll; ayn=e*sin(omega)+aynl; /* solve keplers equation */ capu=fmod(xlt-xnode,TWOPI); temp2=capu; for (i=0;i<10;i++) { sinepw=sin(temp2); cosepw=cos(temp2); temp3=axn*sinepw; temp4=ayn*cosepw; temp5=axn*cosepw; temp6=ayn*sinepw; epw=(capu-temp4+temp3-temp2)/(1.0-temp5-temp6)+temp2; if (fabs(epw-temp2)<=E6A) break; temp2=epw; } /* short period preliminary quantities */ ecose=temp5+temp6; esine=temp3-temp4; elsq=axn*axn+ayn*ayn; temp=1.0-elsq; pl=a*temp; r=a*(1.0-ecose); temp1=1.0/r; rdot=XKE*sqrt(a)*esine*temp1; rfdot=XKE*sqrt(pl)*temp1; temp2=a*temp1; betal=sqrt(temp); temp3=1.0/(1.0+betal); cosu=temp2*(cosepw-axn+ayn*esine*temp3); sinu=temp2*(sinepw-ayn-axn*esine*temp3); u=atan2(sinu,cosu); sin2u=2.0*sinu*cosu; cos2u=2.0*cosu*cosu-1.0; temp=1.0/pl; temp1=CK2*temp; temp2=temp1*temp; /* update for short periodics */ rk=r*(1.0-1.5*temp2*betal*x3thm1)+0.5*temp1*x1mth2*cos2u; uk=u-0.25*temp2*x7thm1*sin2u; xnodek=xnode+1.5*temp2*cosio*sin2u; xinck=xincl+1.5*temp2*cosio*sinio*cos2u; rdotk=rdot-xn*temp1*x1mth2*sin2u; rfdotk=rfdot+xn*temp1*(x1mth2*cos2u+1.5*x3thm1); /* orientation vectors */ sinuk=sin(uk); cosuk=cos(uk); sinik=sin(xinck); cosik=cos(xinck); sinnok=sin(xnodek); cosnok=cos(xnodek); xmx=-sinnok*cosik; xmy=cosnok*cosik; ux=xmx*sinuk+cosnok*cosuk; uy=xmy*sinuk+sinnok*cosuk; uz=sinik*sinuk; vx=xmx*cosuk-cosnok*sinuk; vy=xmy*cosuk-sinnok*sinuk; vz=sinik*cosuk; /* position and velocity */ x=rk*ux; y=rk*uy; z=rk*uz; xdot=rdotk*ux+rfdotk*vx; ydot=rdotk*uy+rfdotk*vy; zdot=rdotk*uz+rfdotk*vz; rs[0]=x*XKMPER/AE*1E3; /* (m) */ rs[1]=y*XKMPER/AE*1E3; rs[2]=z*XKMPER/AE*1E3; rs[3]=xdot*XKMPER/AE*XMNPDA/86400.0*1E3; /* (m/s) */ rs[4]=ydot*XKMPER/AE*XMNPDA/86400.0*1E3; rs[5]=zdot*XKMPER/AE*XMNPDA/86400.0*1E3; } /* drop spaces at string tail ------------------------------------------------*/ static void chop(char *buff) { int i; for (i=strlen(buff)-1;i>=0;i--) { if (buff[i]==' '||buff[i]=='\r'||buff[i]=='\n') buff[i]='\0'; else break; } } /* test TLE line checksum ----------------------------------------------------*/ static int checksum(const char *buff) { int i,cs=0; if (strlen(buff)<69) return 0; for (i=0;i<68;i++) { if ('0'<=buff[i]&&buff[i]<='9') cs+=(int)(buff[i]-'0'); else if (buff[i]=='-') cs+=1; } return (int)(buff[68]-'0')==cs%10; } /* decode TLE line 1 ---------------------------------------------------------*/ static int decode_line1(const char *buff, tled_t *data) { double year,doy,nddot,exp1,bstar,exp2,ep[6]={2000,1,1}; strncpy(data->satno,buff+2,5); /* satellite number */ data->satno[5]='\0'; chop(data->satno); data->satclass=buff[7]; /* satellite classification */ strncpy(data->desig,buff+9,8); /* international designator */ data->desig[8]='\0'; chop(data->desig); year =str2num(buff,18, 2); /* epoch year */ doy =str2num(buff,20,12); /* epoch day of year */ data->ndot=str2num(buff,33,10); /* 1st time derivative of n */ nddot =str2num(buff,44, 6); /* 2nd time derivative of n */ exp1 =str2num(buff,50, 2); bstar =str2num(buff,53, 6); /* Bstar drag term */ exp2 =str2num(buff,59, 2); data->etype=(int)str2num(buff,62,1); /* ephemeris type */ data->eleno=(int)str2num(buff,64,4); /* ephemeris number */ data->nddot=nddot*1E-5*pow(10.0,exp1); data->bstar=bstar*1E-5*pow(10.0,exp2); ep[0]=year+(year<57.0?2000.0:1900.0); data->epoch=timeadd(epoch2time(ep),(doy-1.0)*86400.0); data->inc=data->OMG=data->ecc=data->omg=data->M=data->n=0.0; data->rev=0; return 1; } /* decode TLE line 2 ---------------------------------------------------------*/ static int decode_line2(const char *buff, tled_t *data) { char satno[16]; strncpy(satno,buff+2,5); /* satellite number */ satno[5]='\0'; chop(satno); data->inc=str2num(buff, 8, 8); /* inclination (deg) */ data->OMG=str2num(buff,17, 8); /* RAAN (deg) */ data->ecc=str2num(buff,26, 7)*1E-7; /* eccentricity */ data->omg=str2num(buff,34, 8); /* argument of perigee (deg) */ data->M =str2num(buff,43, 8); /* mean anomaly (deg) */ data->n =str2num(buff,52,11); /* mean motion (rev/day) */ data->rev=(int)str2num(buff,63,5); /* revolution number */ if (strcmp(satno,data->satno)) { trace(2,"tle satno mismatch: %s %s\n",data->satno,satno); return 0; } if (data->n<=0.0||data->ecc<0.0) { trace(2,"tle data error: %s\n",satno); return 0; } return 1; } /* add TLE data --------------------------------------------------------------*/ static int add_data(tle_t *tle, const tled_t *data) { tled_t *tle_data; if (tle->n>=tle->nmax) { tle->nmax=tle->nmax<=0?1024:tle->nmax*2; if (!(tle_data=(tled_t *)realloc(tle->data,sizeof(tled_t)*tle->nmax))) { trace(1,"tle malloc error\n"); free(tle->data); tle->data=NULL; tle->n=tle->nmax=0; return 0; } tle->data=tle_data; } tle->data[tle->n++]=*data; return 1; } /* compare TLE data by satellite name ----------------------------------------*/ static int cmp_tle_data(const void *p1, const void *p2) { const tled_t *q1=(const tled_t *)p1,*q2=(const tled_t *)p2; return strcmp(q1->name,q2->name); } /* read TLE file --------------------------------------------------------------- * read NORAD TLE (two line element) data file (ref [2],[3]) * args : char *file I NORAD TLE data file * tle_t *tle O TLE data * return : status (1:ok,0:error) * notes : before calling the function, the TLE data should be initialized. * the file should be in a two line (only TLE) or three line (satellite * name + TLE) format. * the characters after # in a line are treated as comments. *-----------------------------------------------------------------------------*/ extern int tle_read(const char *file, tle_t *tle) { FILE *fp; tled_t data={{0}}; char *p,buff[256]; int line=0; if (!(fp=fopen(file,"r"))) { trace(2,"tle file open error: %s\n",file); return 0; } while (fgets(buff,sizeof(buff),fp)) { /* delete comments */ if ((p=strchr(buff,'#'))) *p='\0'; chop(buff); if (buff[0]=='1'&&checksum(buff)) { /* decode TLE line 1 */ if (decode_line1(buff,&data)) line=1; } else if (line==1&&buff[0]=='2'&&checksum(buff)) { /* decode TLE line 2 */ if (!decode_line2(buff,&data)) continue; /* add TLE data */ if (!add_data(tle,&data)) { fclose(fp); return 0; } data.name[0]='\0'; data.alias[0]='\0'; } else if (buff[0]) { /* satellite name in three line format */ strcpy(data.alias,buff); /* omit words in parentheses */ if ((p=strchr(data.alias,'('))) *p='\0'; chop(data.alias); line=0; } } fclose(fp); /* sort tle data by satellite name */ if (tle->n>0) qsort(tle->data,tle->n,sizeof(tled_t),cmp_tle_data); return 1; } /* read TLE satellite name file ------------------------------------------------ * read TLE satellite name file * args : char *file I TLE satellite name file * tle_t *tle IO TLE data * return : status (1:ok,0:error) * notes : before calling the function, call tle_read() to read tle table * the TLE satellite name file contains the following record as a text * line. strings after # are treated as comments. * * name satno [desig [# comment]] * * name : satellite name * satno: satellite catalog number * desig: international designator (optional) *-----------------------------------------------------------------------------*/ extern int tle_name_read(const char *file, tle_t *tle) { FILE *fp; tled_t data; char *p,buff[256],name[256],satno[256],desig[256]; int i; if (!(fp=fopen(file,"r"))) { trace(2,"tle satellite name file open error: %s\n",file); return 0; } while (fgets(buff,sizeof(buff),fp)) { if ((p=strchr(buff,'#'))) *p='\0'; desig[0]='\0'; if (sscanf(buff,"%s %s %s",name,satno,desig)<2) continue; satno[5]='\0'; for (i=0;in;i++) { if (!strcmp(tle->data[i].satno,satno)|| !strcmp(tle->data[i].desig,desig)) break; } if (i>=tle->n) { trace(4,"no tle data: satno=%s desig=%s\n",satno,desig); continue; } if (!*tle->data[i].name) { strncpy(tle->data[i].name,name,31); tle->data[i].name[31]='\0'; } else { data=tle->data[i]; strncpy(data.name,name,31); data.name[31]='\0'; if (!add_data(tle,&data)) { break; } } } fclose(fp); /* sort tle data by satellite name */ if (tle->n>0) qsort(tle->data,tle->n,sizeof(tled_t),cmp_tle_data); return 1; } /* satellite position and velocity with TLE data ------------------------------- * compute satellite position and velocity in ECEF with TLE data * args : gtime_t time I time (GPST) * char *name I satellite name ("": not specified) * char *satno I satellite catalog number ("": not specified) * char *desig I international designaor ("": not specified) * tle_t *tle I TLE data * erp_t *erp I EOP data (NULL: not used) * double *rs O sat position/velocity {x,y,z,vx,vy,vz} (m,m/s) * return : status (1:ok,0:error) * notes : the coordinates of the position and velocity are ECEF (ITRF) * if erp == NULL, polar motion and ut1-utc are neglected *-----------------------------------------------------------------------------*/ extern int tle_pos(gtime_t time, const char *name, const char *satno, const char *desig, const tle_t *tle, const erp_t *erp, double *rs) { gtime_t tutc; double tsince,rs_tle[6],rs_pef[6],gmst; double R1[9]={0},R2[9]={0},R3[9]={0},W[9],erpv[5]={0}; int i=0,j,k,stat=1; /* binary search by satellite name */ if (*name) { for (i=j=0,k=tle->n-1;j<=k;) { i=(j+k)/2; if (!(stat=strcmp(name,tle->data[i].name))) break; if (stat<0) k=i-1; else j=i+1; } } /* serial search by catalog no or international designator */ if (stat&&(*satno||*desig)) { for (i=0;in;i++) { if (!strcmp(tle->data[i].satno,satno)|| !strcmp(tle->data[i].desig,desig)) break; } if (in) stat=0; } if (stat) { trace(4,"no tle data: name=%s satno=%s desig=%s\n",name,satno,desig); return 0; } tutc=gpst2utc(time); /* time since epoch (min) */ tsince=timediff(tutc,tle->data[i].epoch)/60.0; /* SGP4 model propagator by STR#3 */ SGP4_STR3(tsince,tle->data+i,rs_tle); /* erp values */ if (erp) geterp(erp,time,erpv); /* GMST (rad) */ gmst=utc2gmst(tutc,erpv[2]); /* TEME (true equator, mean eqinox) -> ECEF (ref [2] IID, Appendix C) */ R1[0]=1.0; R1[4]=R1[8]=cos(-erpv[1]); R1[7]=sin(-erpv[1]); R1[5]=-R1[7]; R2[4]=1.0; R2[0]=R2[8]=cos(-erpv[0]); R2[2]=sin(-erpv[0]); R2[6]=-R2[2]; R3[8]=1.0; R3[0]=R3[4]=cos(gmst); R3[3]=sin(gmst); R3[1]=-R3[3]; matmul("NN",3,1,3,1.0,R3,rs_tle ,0.0,rs_pef ); matmul("NN",3,1,3,1.0,R3,rs_tle+3,0.0,rs_pef+3); rs_pef[3]+=OMGE*rs_pef[1]; rs_pef[4]-=OMGE*rs_pef[0]; matmul("NN",3,3,3,1.0,R1,R2,0.0,W); matmul("NN",3,1,3,1.0,W,rs_pef ,0.0,rs ); matmul("NN",3,1,3,1.0,W,rs_pef+3,0.0,rs+3); return 1; }