broad.c

国内外搜集的GPS程序代码

broad.c

void broadcast(fp,ifp,borb,itborb)
/* 函数 broadcast 计算广播星历 */
FILE *fp[Ista];
int ifp;
long itborb[8];
double borb[Isat*I96*8];
{    int idat[Isat*6*Iepoch*Ista],idimension=0,version;
     double ddat[Isat*28*Iepoch*Ista],ion_p[8];
     int i,j,K,k,c,c1,j1;
/* 函数 r_eph 读全部星历数据 */ 
     r_eph(fp,stdout,ifp,&version,ion_p,idat,ddat,&idimension);
     for(i=0;i<ifp;i++) fclose(fp[i]);
/* 计算轨道数据 */
     borbc(idat,ddat,idimension,borb,itborb);
/* 为控制的输出 */
}
void borbc(idat,ddat,idimension,borb,itborb)
/* 应用广播星历计算轨道数据 */
int idat[Isat*6*Iepoch*Ista],idimension;
long itborb[8];
double ddat[Isat*28*Iepoch*Ista];
double borb[Isat*I96*8];
{    int i,i1,j,k,j4,Idt=900,iepoch,isat_n,iiv[Isat];
     double epocht,orbit[5];
/* 设置计算时间 */
     for(i=1;i<6;i++) itborb[i]=idat[i]; itborb[5]=0; itborb[6]=0;
     itborb[4] -= 2; if(itborb[4]<0) itborb[4] = 0;
     iepoch = 1 + (-itborb[4]*3600-itborb[5]*60+
            (idat[4+(idimension-1)*6]+2)*3600+idat[5+(idimension-1)*6]*60)/Idt;
     if(iepoch>97) iepoch=97;
     itborb[7]=iepoch;
     for(k=0,j=0;j<iepoch;j++) 				/* 对于每一个历元 */
      { epocht=itborb[4]*3600.+j*Idt;
        isat_n = findisat(idat,ddat,idimension,iiv,epocht);
        if(j==0) itborb[0] = isat_n;
        i = (int)(epocht/3600);j4=(int)(epocht-i*3600);j4=j4/60;
        for(i=0;i<isat_n;i++) 				/* 对于每一卫星 */
         { i1=iiv[i];
           broadcast_orbit(idat,ddat,idimension,i1,epocht,orbit);
           for(j4=0;j4<4;j4++) borb[j4+i*8+j*8*Isat]=orbit[j4];
           borb[7+i*8+j*8*Isat]=orbit[4];
           broadcast_orbit(idat,ddat,idimension,i1,epocht+1.,orbit);
           for(j4=1;j4<4;j4++)
           borb[j4+3+i*8+j*8*Isat]=orbit[j4]-borb[j4+i*8+j*8*Isat];
           k+=1;}}}
int findisat(idat,ddat,idimension,iiv,epocht)
/* 找到共同的卫星号和最接近历元epocht （记录数据）时的卫星向量 iiv */
int idat[Isat*6*Iepoch*Ista],idimension,iiv[Isat];
double ddat[Isat*28*Iepoch*Ista],epocht;
{    int isat=0,i,j1,j2,j3,k,k1;
     double t,tj[Isat*20];
     int isatv[Isat*20],j[Isat*20];
/* 注明卫星号和数据行号以及时间差 */
     for(k=0,i=0;i<idimension;i++) 
       { t = fabs(idat[4+i*6]*3600.-epocht);
         tj[k]=t;j[k]=i;isatv[k]=idat[i*6];k+=1; }
/* 找到最靠近的数据 */
     onceagain:;
     for(j1=0;j1<k-1;j1++) 
       { for(j2=j1+1;j2<k;j2++) 
          { if(isatv[j1]==isatv[j2]) 
             { if(tj[j1]>tj[j2]+0.001) j3 = j1;
                else j3 = j2;
                for(k1=j3;k1<k-1;k1++) 
                  { tj[k1]=tj[k1+1];isatv[k1]=isatv[k1+1];j[k1]=j[k1+1]; }
                 k -= 1;
                 goto onceagain; }}}
/* 将数据排次序，令卫星号增加 */
     once1again:; j3 = 0;
     for(j1=0;j1<k-1;j1++) 
      { for(j2=j1+1;j2<k;j2++) 
        { if(isatv[j1]>isatv[j2]) 
          { j3 += 1;
            i=isatv[j1];isatv[j1]=isatv[j1+1];isatv[j1+1]=i;
            i=j[j1],j[j1]=j[j1+1];j[j1+1]=i; } } }
     if(j3!=0) goto once1again;   
/* 将结果放入输出向量 */ 
     for(i=0;i<k;i++) {iiv[i] = j[i];}
     return(k);}
void broadcast_orbit(idat,ddat,idimension,i_line,epocht,orbit)
/* 此函数应用广播星历的i_line 数据计算epocht历元的卫星位置 */
int idat[Isat*6*Iepoch*Ista],idimension,i_line;
double ddat[Isat*28*Iepoch*Ista];
double orbit[5],epocht;
{    int i,j,j1,k;
     double e,M_,l,w,r,ri,E,u,v,mue,we,to,toe,sqrta,r0;
     double sw,cw,sl,cl,si,ci,rx,ry,rz,pi2,tk,u2,t;
     mue = 39860.05e+10; we = 729211.5e-10; pi2 = 2*acos(-1.);
     t = epocht;
     i = i_line; 
     toe = ddat[12+i*28]; sqrta = ddat[11+i*28];
     to = toe-(idat[4+i*6]*3600+idat[5+i*6]*60+ddat[0+i*28]);
     if(to>302400.e0) to -= 604800.e0;
     if(to<-302400.e0) to += 604800.e0;
     t += to;
     tk = t -toe;
     if(tk>302400.e0) tk -= 604800.e0;
     if(tk<-302400.e0) tk += 604800.e0;
     M_ = ddat[7+i*28] +
         (sqrt(mue)/ddat[11+i*28]/ddat[11+i*28]/ddat[11+i*28]+ddat[6+i*28])*tk;
     M_ = fmod(M_,pi2);
     M_ = fmod(M_+pi2,pi2);
     e = ddat[9+i*28];
     E = Kepler_equation(e,M_);
     v = atan2(sqrt(1-e*e)*sin(E),cos(E)-e);
     u = ddat[18+i*28] + v;
     u2 = fmod(u+u,pi2);
     u2 = fmod(u2+pi2,pi2);
     w = u + ddat[8+i*28]*cos(u2) + ddat[10+i*28]*sin(u2);
     w = fmod(w,pi2);
     w = fmod(w+pi2,pi2);
     r0 = sqrta*(1-e*cos(E))*sqrta;
     r = r0 + ddat[17+i*28]*cos(u2) + ddat[5+i*28]*sin(u2);
     ri = ddat[16+i*28] + ddat[13+i*28]*cos(u2) + ddat[15+i*28]*sin(u2)+ddat[20+i*28]*tk;
     l = ddat[14+i*28] + (ddat[19+i*28]-we)*tk - we*fmod(toe,6.048e5);
     l = fmod(l, pi2);
     l = fmod(l+pi2, pi2);
     cw = cos(w); sw = sin(w);
     cl = cos(l); sl = sin(l);
     ci = cos(ri); si = sin(ri);
     rx =  r*cw*cl-r*sw*sl*ci;
     ry =  r*cw*sl+r*sw*cl*ci;
     rz =  r*sw*si;
     orbit[0]=(double)idat[0+i*6];
     orbit[1]=rx;orbit[2]=ry;orbit[3]=rz;
     orbit[4] = ddat[1+i*28] + ddat[2+i*28]*tk + ddat[3+i*28]*tk*tk;
     orbit[4]*=1000000; }
double Kepler_equation(e,M_)
double e,M_;
/* 求解开普勒方程，求 E(t) = M(t) + e*sinE(t)  */
  { int i;
    double s,s1;
    s = M_; 
    repeat:;
    s1 = M_ + e*sin(s);
    if(fabs(s1-s)>1.e-8) {s=s1;goto repeat;}
    return(s1);}
void r_eph(fp,fp1,ifp,version,ion_p,idat,ddat,idimension) 
/* 函数 r_eph 从全部有关数据中读取标准轨道文件并检查数据，避免重复*/
FILE *fp[Ista],*fp1;
int *version,*idimension;
double ion_p[8];
int ifp,idat[Isat*6*Iepoch*Ista];
double ddat[Isat*28*Iepoch*Ista];
{  int i,j,K=0,jj,c,Ic;
   int sat,year,month,day,hour,min;
   double f1,f2,f3,f4;
   char s[Iline];
/*读广播轨道数据文件中的字头 */ 
     for(jj=0;jj<ifp;jj++) 
      { c = getline(fp[jj],s);         
        sscanf(s,"%d", &i); *version = i; Ic = i;
        for (j=0;j<2;j++) c = getline(fp[jj],s); 
        if(i==2) 
        { for (j=0;j<4;j++) 
          { c = getline(fp[jj],s);
           if(strstr(s,"HEAD")) goto datapart; }}
        datapart:;
/* 读取轨道数据 */
        while ((c = getline(fp[jj],s)) != EOF) 
        {  i = 0; DdE_e(s);
           sscanf(s,"%d %d %d %d %d %d %lf %lf %lf %lf",&sat,&year,&month,
                  &day,&hour,&min,&f1,&f2,&f3,&f4);
/* 时间检查 小时=24，分钟=60，秒数=60 */
           if(f1==60.) { f1 = 0.; min += 1; }
           if(min==60) { min = 0; hour += 1; }
           if(hour==24) { hour = 0; day += 1; }
/* 存储数据 */
           idat[0+K*6] = sat;   idat[1+K*6] = year;
           idat[2+K*6] = month; idat[3+K*6] = day;
           idat[4+K*6] = hour;  idat[5+K*6] = min;
           ddat[0+K*28] = f1;   ddat[1+K*28] = f2;
           ddat[2+K*28] = f3;   ddat[3+K*28] = f4;
/* 读其他的6行并存储 */
           for (j=0;j<6;j++) 
            { c = getline(fp[jj],s); i += 1;  DdE_e(s);
              sscanf(s,"%lf %lf %lf %lf",&f1,&f2,&f3,&f4);
              ddat[0+i*4+K*28] = f1;   ddat[1+i*4+K*28] = f2;
              ddat[2+i*4+K*28] = f3;   ddat[3+i*4+K*28] = f4; }
           if(Ic==2) c = getline(fp[jj],s);
           K += 1; }
        *idimension = K;
/* 数据检查 */
        eph_check(fp1,version,ion_p,idat,ddat,idimension); }
/* 为控制的输出 */
}
void eph_check(fp1,version,ion_p,idat,ddat,idimension) 
/* 函数 eph_check 检查由 r_eph 读取的星历数据，按时间和卫星号排序，避免重复 */
FILE *fp1;
int *version,*idimension;
double ion_p[8];
int idat[Isat*6*Iepoch*Ista];
double ddat[Isat*28*Iepoch*Ista];
{    int i,j,k,K,c,c1;
     int isum;
     long time,time1;
     double f1,f2,f3,f4;
     K = *idimension;
/* 将数据按时间排序 */
     tloop:; c = 0;
     for (i=0;i<K-1;i++) 
      { time = idat[2+i*6]*10000+idat[3+6*i]*100+idat[4+6*i];
        time1 = idat[2+i*6+6]*10000+idat[3+6*i+6]*100+idat[4+6*i+6];
        if (time>time1) 
        { c += 1;
          for (j=0;j<6;j++) 
            { isum = idat[j+6*i];
              idat[j+6*i] = idat[j+6*i+6]; idat[j+6*i+6] = isum; }
              for (j=0;j<28;j++) 
             { f1 = ddat[j+28*i];
               ddat[j+28*i] = ddat[j+28*i+28]; ddat[j+28*i+28] = f1;}}
/* 在同一历元中，将数据按卫星号排序 */ 
        else if (time==time1) 
        { if (idat[6*i]>idat[6+6*i]) 
          { c += 1;
            for (j=0;j<6;j++) 
            {  isum = idat[j+6*i];
               idat[j+6*i] = idat[j+6*i+6]; idat[j+6*i+6] = isum; }
               for (j=0;j<28;j++) 
               { f1 = ddat[j+28*i];
                 ddat[j+28*i] = ddat[j+28*i+28]; ddat[j+28*i+28] = f1;}}
/* 对于同样卫星和同一历元，如果数据相同，进行检查 */ 
           else if (idat[6*i]==idat[6+6*i]) 
          { c1 = 0; for (j=0;j<6;j++) 
           { if (idat[j+6*i]!=idat[j+6*i+6]) c1 += 1; }
              for (j=0;j<28;j++) 
              { if (ddat[j+28*i]!=ddat[j+28*i+28]) c1 += 1; }
              c1 = 0; 
/* 如果数据不同，则存储，删除相同历元和相同卫星号的数据 */
              if (c1==0) 
              {  for (k=i+1;k<K-1;k++)
                {  for (j=0;j<6;j++) idat[j+6*k]=idat[j+6*k+6];
                    for (j=0;j<28;j++) ddat[j+28*k]=ddat[j+28*k+28]; }
                 K -= 1; *idimension = K;
                 goto tloop; }
              else {;}   }}}
     if (c!=0) goto tloop;}
void DdE_e(s)
/* 改变数据指数中格式 D, d, E 为 e */
char s[Iline];
{    int i,c;
     for (i=0;i<Iline;i++) 
    {   c = s[i]; if(c=='D' || c=='d' || c=='E') s[i] = 'e';
        if (c=='\n') break;  }
}