C++卫星定位程序--通过卫星星历确定卫星位置

#include<stdio.h> #include<stdlib.h> #include<conio.h> #include<math.h> void main() { FILE*fpn; if((fpn=fopen("e:\\08337068melody\\gnss\\51.97n","r"))==NULL) { printf("cannot open this file\n"); exit(0); } FILE*fpo; if((fpo=fopen("e:\\08337068melody\\gnss\\51.97o","r"))==NULL) { printf("cannot open this file\n"); exit(0); } /*打开文件*/ struct STL { int prn,year,month,day,hour,minute,num,prn1,prn2,prn3,prn4,prn5,prn6; double second,af0,af1,af2,aode,crs,dn,M0,cuc,e,cus,rA,toe,cic,omg0,cis,i0,crc,w,omgdot,idot,cflg,weekno,pflg,svacc,svhlth,tgd,aodc,ttm,phi,empty1,empty2,p1,p2,p3,p4,p5,p6; }satellite[6],satellite2[117]; int aa,bb,dd,prn,year,month,day,hour,minute,second; char str[100]; double t,xx,yy,zz,p0,wk,w=7.292115147e-5,toc; double GM=3.986005e14,n0,n,tk,mk,ek1,ek2,ek,error=1.0e-12,vk,phik,du,di,dr,uk,rk,ik,xk,yk; double we=7.292115147e-5,c=2.99792458e8; /*定义变量*/ for(dd=0;dd<4;dd++)fgets(str,100,fpn); for(aa=0;aa<6;aa++) { fscanf(fpn,"%d%d%d%d%d%d%le%le%le%le\n",&satellite[aa].prn,&satellite[aa].year,&satellite[aa].month,&satellite[aa].day,&satellite[aa].hour,&satellite[aa].minute,&satellite[aa].second,&satellite[aa].af0,&satellite[aa].af1,&satellite[aa].af2); fscanf(fpn,"%le%le%le%le\n",&satellite[aa].aode,&satellite[aa].crs,&satellite[aa].dn,&satellite[aa].M0); fscanf(fpn,"%le%le%le%le\n",&satellite[aa].cuc,&satellite[aa].e,&satellite[aa].cus,&satellite[aa].rA); fscanf(fpn,"%le%le%le%le\n",&satellite[aa].toe,&satellite[aa].cic,&satellite[aa].omg0,&satellite[aa].cis); fscanf(fpn,"%le%le%le%le\n",&satellite[aa].i0,&satellite[aa].crc,&satellite[aa].w,&satellite[aa].omgdot); fscanf(fpn,"%le%le%le%le\n",&satellite[aa].idot,&satellite[aa].cflg,&satellite[aa].weekno,&satellite[aa].pflg); fscanf(fpn,"%le%le%le%le\n",&satellite[aa].svacc,&satellite[aa].svhlth,&satellite[aa].tgd,&satellite[aa].aodc); fscanf(fpn,"%le%le%le%le\n",&satellite[aa].ttm,&satellite[aa].phi,&satellite[aa].empty1,&satellite[aa].empty2); } /*读N文件*/ for(dd=0;dd<21;dd++)fgets(str,100,fpo); for(bb=0;bb<11;bb++) { fscanf(fpo,"%d%d%d%d%d%lf",&satellite2[bb].year,&satellite2[bb].month,&satellite2[bb].day,&satellite2[bb].hour,&satellite2[bb].minute,&satellite2[bb].second); fgets(str,6,fpo); fscanf(fpo,"%dG",&satellite2[bb].num); if(satellite2[bb].num==5) { fscanf(fpo,"%dG",&satellite2[bb].prn1); fscanf(fpo,"%dG",&satellite2[bb].prn2); fscanf(fpo,"%dG",&satellite2[bb].prn3); fscanf(fpo,"%dG",&satellite2[bb].prn4); fscanf(fpo,"%dG",&satellite2[bb].prn5); fseek(fpo,4,1); fscanf(fpo,"%lf ",&satellite2[bb].p1); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p2); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p3); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p4); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p5); fseek(fpo,64,1); } if(satellite2[bb].num==6) { fscanf(fpo,"%dG",&satellite2[bb].prn1); fscanf(fpo,"%dG",&satellite2[bb].prn2); fscanf(fpo,"%dG",&satellite2[bb].prn3); fscanf(fpo,"%dG",&satellite2[bb].prn4); fscanf(fpo,"%dG",&satellite2[bb].prn5); fscanf(fpo,"%dG",&satellite2[bb].prn6); fseek(fpo,4,1); fscanf(fpo,"%lf ",&satellite2[bb].p1); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p2); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p3); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p4); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p5); fseek(fpo,66,1); fscanf(fpo,"%lf ",&satellite2[bb].p6); fseek(fpo,64,1); }/*读O文件*/ year=1900+satellite2[bb].year; month=satellite2[bb].month; day=satellite2[bb].day; hour=satellite2[bb].hour; minute=satellite2[bb].minute; second=int(satellite2[bb].second); printf("%d-%d-%d %d:%d:%d\n",year,month,day,hour,minute,second);/*输出GPS时*/ satellite2[bb].day=satellite2[bb].day+int((satellite2[bb].year-80)/4)+1;/*考虑闰年的影响*/ if(satellite2[bb].month==1)satellite2[bb].month=0; if(satellite2[bb].month==2)satellite2[bb].month=31; if(satellite2[bb].month==3)satellite2[bb].month=59; if(satellite2[bb].month==4)satellite2[bb].month=90; if(satellite2[bb].month==5)satellite2[bb].month=120; if(satellite2[bb].month==6)satellite2[bb].month=151; if(satellite2[bb].month==7)satellite2[bb].month=181; if(satellite2[bb].month==8)satellite2[bb].month=212; if(satellite2[bb].month==9)satellite2[bb].month=243; if(satellite2[bb].month==10)satellite2[bb].month=273; if(satellite2[bb].month==11)satellite2[bb].month=304; if(satellite2[bb].month==12)satellite2[bb].month=334;/*把月转为以天为单位*/ t=(((satellite2[bb].year-80)*365+satellite2[bb].month+satellite2[bb].day-5)%7)*86400+satellite2[bb].hour*3600+(satellite2[bb].minute-4)*60+satellite2[bb].second;/*计算GPS时*/ /*计算卫星坐标*/ if(satellite2[bb].num==5) { prn=satellite2[bb].prn1; printf("%d\t",prn); for(aa=0;aa<6;aa++) { if(satellite[aa].prn==prn) { n0=sqrt(GM)/(satellite[aa].rA*satellite[aa].rA*satellite[aa].rA); n=n0+satellite[aa].dn; //t=t-satellite2[bb].p1/c; tk=t-satellite[aa].toe; if(tk>302400)tk=tk-604800; if(tk<-302400)tk=tk+604800; mk=satellite[aa].M0+n*tk; ek1=mk; do { ek2=mk+satellite[aa].e*sin(ek1); if(fabs(ek2-ek1)<=error)break; ek1=ek2; } while(1); ek=ek1; vk=atan2((sqrt(1-satellite[aa].e*satellite[aa].e)*sin(ek)),(cos(ek)-satellite[aa].e)); phik=vk+satellite[aa].w; du=satellite[aa].cuc*cos(2.0*phik)+satellite[aa].cus*sin(2.0*phik); dr=satellite[aa].crc*cos(2.0*phik)+satellite[aa].crs*sin(2.0*phik); di=satellite[aa].cic*cos(2.0*phik)+satellite[aa].cis*sin(2.0*phik); uk=phik+du; rk=satellite[aa].rA*satellite[aa].rA*(1.0-satellite[aa].e*cos(ek))+dr; ik=satellite[aa].i0+di+satellite[aa].idot*tk; xk=rk*cos(uk); yk=rk*sin(uk); wk=satellite[aa].omg0+(satellite[aa].omgdot-w)*tk-w*satellite[aa].toe; xx=xk*cos(wk)-yk*cos(ik)*sin(wk); yy=xk*sin(wk)+yk*cos(ik)*cos(wk); zz=yk*sin(ik); p0=satellite2[bb].p1; printf("%9.3lf\t%9.3lf\t%9.3lf\t%9.3lf\n",xx,yy,zz,p0); } }/*第一颗*/ prn=satellite2[bb].prn2; printf("%d\t",prn); for(aa=0;aa<6;aa++) { if(satellite[aa].prn==prn) { n0=sqrt(GM)/(satellite[aa].rA*satellite[aa].rA*satellite[aa].rA); n=n0+satellite[aa].dn; //t=t-satellite2[bb].p2/c; tk=t-satellite[aa].toe; if(tk>302400)tk=tk-604800; if(tk<-302400)tk=tk+604800; mk=satellite[aa].M0+n*tk; ek1=mk; do { ek2=mk+satellite[aa].e*sin(ek1); if(fabs(ek2-ek1)<=error)break; ek1=ek2; } while(1); ek=ek1; vk=atan2((sqrt(1-satellite[aa].e*satellite[aa].e)*sin(ek)),(cos(ek)-satellite[aa].e)); phik=vk+satellite[aa].w; du=satellite[aa].cuc*cos(2.0*phik)+satellite[aa].cus*sin(2.0*phik); dr=satellite[aa].crc*cos(2.0*phik)+satellite[aa].crs*sin(2.0*phik); di=satellite[aa].cic*cos(2.0*phik)+satellite[aa].cis*sin(2.0*phik); uk=phik+du; rk=satellite[aa].rA*satellite[aa].rA*(1.0-satellite[aa].e*cos(ek))+dr; ik=satellite[aa].i0+di+satellite[aa].idot*tk; xk=rk*cos(uk); yk=rk*sin(uk); wk=satellite[aa].omg0+(satellite[aa].omgdot-w)*tk-w*satellite[aa].toe; xx=xk*cos(wk)-yk*cos(ik)*sin(wk); yy=xk*sin(wk)-yk*cos(ik)*cos(wk); zz=yk*sin(ik); p0=satellite2[bb].p2; printf("%9.3lf\t%9.3lf\t%9.3lf\t%9.3lf\n",xx,yy,zz,p0); } }/*第二颗*/ prn=satellite2[bb].prn3; printf("%d\t",prn); for(aa=0;aa<6;aa++) { if(satellite[aa].prn==prn) { n0=sqrt(GM)/(satellite[aa].rA*satellite[aa].rA*satellite[aa].rA); n=n0+satellite[aa].dn; //t=t-satelli