SAT_POS.rar_GPS 定位_GPS卫星定位_get_sat_pos_卫星_卫星定位

#include <stdio.h> #include <stdlib.h> #include "rinex.h" #include "sv.h" void WriteSatPosFile(FILE *SatPosFile,int prn,XYZCoor *SVPos); void GetSVPos(SVText *Text,double t,double *X,double *Y,double *Z); double Get_atan(double z,double y); int GetGPSTime(int year,int month,int day, int hour,int minute,double second,double *gpstime); int read_RinexEPP( FILE *RinexEPP_file,SVText *snv, int *year, int *month,int *day,int *hour,int *minute, double *second,double *gpstime,int *wn); void main() { char RinexEPPName[20]; FILE *RinexEPP_file, *SatPosFile; double SVPosX,SVPosY,SVPosZ; int prn,i; SVText snv; char temp[200]; int wn,year,month,day,hour, minute; double second,gpstime; printf("Please Input Rinex Nav File Name: "); scanf("%s",RinexEPPName); if ((RinexEPP_file = fopen(RinexEPPName, "rt")) == NULL) { fprintf(stderr, "Cannot open input file.\n"); exit(1); } if ((SatPosFile = fopen("satpos.out", "w"))== NULL) { fprintf(stderr, "Cannot open outut file.\n"); exit(1); } rewind(RinexEPP_file); for(i=0;i<3;i++) { fgets(temp,200,RinexEPP_file); } do { if(read_RinexEPP(RinexEPP_file,&snv, &year,&month,&day,&hour,&minute,&second,&gpstime,&wn) ) break; /* compute satellite position */ GetSVPos(&snv,gpstime,&SVPosX,&SVPosY,&SVPosZ); /* write satellite position file */ fprintf(SatPosFile,"%3i%19.11e%19.11e%19.11e\n", snv.prn,SVPosX,SVPosY,SVPosZ); }while(1); fclose(RinexEPP_file); fclose(SatPosFile); } /*********************** read_EPP() *************************/ int read_RinexEPP( FILE *RinexEPP_file,SVText *snv, int *year, int *month,int *day,int *hour,int *minute, double *second,double *gpstime,int *wn) { int j; char t1[30],t2[30],t3[30],t4[30],t5[30]; /* PRN /EPOCH /SV CLK */ if(fscanf(RinexEPP_file,"%d%d%d%d%d%d%s%s%s%s\n", &snv->prn,year,month,day, hour,minute,t1,t2,t3,t4)==EOF) return 1; *second=atof(t1); snv->af0=atof(t2); snv->af1=atof(t3); snv->af2=atof(t4); *year+=1900; *wn=snv->wn = GetGPSTime(*year,*month,*day,*hour,*minute,*second, gpstime); snv->tow = (long) gpstime; /* BROADCAST ORBIT -1 */ if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4) ==EOF) return 1; snv->aode=atof(t1); snv->crs=atof(t2); snv->deltan=atof(t3); snv->m0=atof(t4); /* BROADCAST ORBIT -2 */ if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4) ==EOF) return 1; snv->cuc=atof(t1); snv->e=atof(t2); snv->cus=atof(t3); snv->roota=atof(t4); /* BROADCAST ORBIT -3 */ if(fscanf( RinexEPP_file,"%s%s%s%s\n", t1,t2,t3,t4) ==EOF) return 1; snv->toe=atof(t1); snv->cic=atof(t2); snv->omega0=atof(t3); snv->cis=atof(t4); /* BROADCAST ORBIT -4 */ if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4) ==EOF) return 1 ; snv->i0=atof(t1); snv->crc=atof(t2); snv->omega=atof(t3); snv->omegadot=atof(t4); /* BROADCAST ORBIT -5 */ if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4) ==EOF) return 1 ; snv->idot=atof(t1); snv->wn=atof(t3); /* BROADCAST ORBIT -6 */ if(fscanf( RinexEPP_file,"%s%s%s%s\n",t1,t2,t3,t4) ==EOF) return 1 ; snv->tgd=atof(t3); return(0); } /* Using the datas from SV navigation Text to compute the SV position in ECEFCoor */ void GetSVPos(SVText *Text,double t,double *X,double *Y,double *Z) { double mu=3.986005e14; /* Earth constant , unit m3/s2 */ double n0,n; /* mean rate of angle */ double tk, mk, ek1, ek2, ek; double Error=1.0e-12; double vk, phik; double deltau,deltar,deltai; double uk, rk, ik; double xk, yk; double omegak; double omegae=7.292115147e-5; /* earth self round rate of angle */ /* compute SV mean rate of angle */ n0 = sqrt(mu) / (Text->roota * Text->roota * Text->roota); n = n0 + Text->deltan ; /* compute time tk */ tk = t - Text->toe; if(tk > 302400) tk -= 604800; else if(tk < -302400) tk +=604800; /* compute mean anomoly at measure time */ mk = Text->m0 + n * tk; /* computer ek */ ek1 = mk; do { ek2 = mk + Text->e * sin(ek1); if(fabs(ek2-ek1)<=Error ) break; ek1=ek2; }while(1); ek=ek1; /* computer vk */ /* vk = atan(sqrt(1.0-Text->e *Text->e)*sin(ek)) / (cos(ek)-Text->e);*/ vk = Get_atan(cos(ek)-Text->e,sqrt(1.0-Text->e *Text->e)*sin(ek)); /* compute phik */ phik = vk + Text->omega ; /* compute deltau,deltar,deltai */ deltau = Text->cuc * cos(2.0*phik) + Text->cus *sin(2.0*phik) ; deltar = Text->crc * cos(2.0*phik) + Text->crs *sin(2.0*phik) ; deltai = Text->cic * cos(2.0*phik) + Text->cis *sin(2.0*phik) ; /* computer uk, rk and ik */ uk = phik +deltau; rk = Text->roota *Text->roota * (1.0-Text->e * cos(ek)) + deltar ; ik = Text->i0 + deltai + Text->idot * tk ; /* compute xk,yk */ xk = rk * cos(uk); yk = rk * sin(uk); /* compute omegak */ omegak = Text->omega0 + (Text->omegadot - omegae)*tk -omegae * Text->toe ; /* compute ECEF */ *X = xk * cos(omegak) - yk * cos(ik) *sin(omegak) ; *Y = xk * sin(omegak) + yk * cos(ik) *cos(omegak) ; *Z = yk*sin(ik) ; } /* End of Function */ /* Caculate x=atan(y/z) */ double Get_atan(double z,double y) { double x; if (z==0) x=M_PI/2; else{ if (y==0) x=M_PI; else{ x=atan(fabs(y/z)); if ((y>0)&&(z<0)) x=M_PI-x; else if ((y<0)&&(z<0)) x=M_PI+x; else if((y<0)&&(z>0)) x=2*M_PI-x; } } return x; } /********************************************** Convert date and time to GPS time ***********************************************/ int GetGPSTime(int year,int month,int day, int hour,int minute,double second,double *gpstime) { int dayofw,dayofy, yr, ttlday, m, weekno; /* Check limits of day, month and year */ if (year < 1981 || month < 1 || month > 12 || day < 1 || day > 31) weekno = 0; /* Convert day, month and year to day of year */ if (month == 1) dayofy = day; else { dayofy = 0; for (m=1; m<=(month-1); m++) { dayofy += dinmth[m]; if ( m==2 ) { if (year % 4 == 0 && year % 100 != 0 || year % 400 == 0) dayofy += 1; } } dayofy += day; } /* Convert day of year and year into week number and day of week */ ttlday = 360; for (yr=1981; yr<=(year-1); yr++) { ttlday += 365; if (yr % 4 == 0 && yr % 100 != 0 || yr % 400 ==0) ttlday += 1; } ttlday += dayofy; weekno = ttlday/7; dayofw = ttlday - 7 * weekno; *gpstime = (hour * 3600 + minute * 60 + second + dayofw * 86400); return weekno; }