gpsfuncs.cpp

开源GPS源码

	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&tot);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&WNt);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&WNlsf);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&DN);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&dtlsf);
	}
     }
     fclose(in);
}

/*******************************************************************************
FUNCTION read_almanac(void)
RETURNS  None.

PARAMETERS None.

PURPOSE
	This function reads the almanac parameters from "current.alm" which is
	created by the program when the data has been read from the satellites

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

void read_almanac(void)
{
  int   id,health,week;
  float eccen,rinc,rras,sqra;
  float ratoa,aopg,rma,af0,af1,toa;
  char text[27];


  almanac_valid=0;
  if ((in = fopen("current.alm", "rt")) == NULL)
  {
	  printf("Cannot open currrent.alm file.\n");
	  for (id=1;id<=32;id++)
	  {
		  gps_alm[id].week=gps_week%1024-1;
		  gps_alm[id].inc=1.0;
	  }
//     almanac_valid=0;
  }
  else
  {
	  status=warm_start;
	  handle=fileno(in);
	  while (!feof(in))
	  {
		  fscanf(in,"%45c",&header);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%d",&id);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%i",&health);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&eccen);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&toa);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&rinc);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&rras);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&sqra);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&ratoa);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&aopg);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&rma);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&af0);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%f",&af1);
	     fscanf(in,"%27c",&text);
	     fscanf(in,"%i",&week);
	     fscanf(in,"%c",&trailer);
	     gps_alm[id].health=health;
	     gps_alm[id].week=week;
	     gps_alm[id].toa=toa;
	     gps_alm[id].ety=eccen;
	     gps_alm[id].toa=toa;
	     gps_alm[id].inc=rinc;
	     gps_alm[id].rra=rras;
	     gps_alm[id].sqa=sqra;
	     gps_alm[id].lan=ratoa;
	     gps_alm[id].aop=aopg;
	     gps_alm[id].ma=rma;
	     gps_alm[id].af0=af0;
	     gps_alm[id].af1=af1;
	     gps_alm[id].sat_file=0;
	     if (gps_alm[id].sqa>0.0) gps_alm[id].w=19964981.84/pow(gps_alm[id].sqa,3);
      }
      fclose(in);
	   alm_gps_week=week;
      alm_toa=toa;
    }
}

/*******************************************************************************
FUNCTION read_ephemeris(void)
RETURNS  None.

PARAMETERS None.

PURPOSE
	This function reads the ephemeris parameters from "current.eph" which is
	created by the program when the data has been read from the satellites

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

void read_ephemeris()
{
	  int   id,health,week,ura,iode,iodc;
      double  toc,toe;
	  double crc,crs,cic,cis,cuc,cus,tgd,ety,inc0,omegadot,w0,w,ma,dn,idot;
      double daf0,daf1,daf2,esqra;
      double d_toe;
      char text[27];

	  if ((in = fopen("current.eph", "rt")) == NULL)
     {
       printf("Cannot open currrent.eph file.\n");
     }
     else
     {
       handle=fileno(in);
       while (!feof(in))
       {
	 fscanf(in,"%37c",&header);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&id);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&health);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&week);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&ura);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&toe);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&iode);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&toc);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%i",&iodc);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&tgd);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&daf0);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&daf1);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&daf2);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&ety);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&inc0);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&idot);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&omegadot);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&esqra);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&dn);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&w0);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&w);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&ma);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&cuc);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&cus);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&crc);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&crs);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&cic);
	 fscanf(in,"%27c",&text);
	 fscanf(in,"%le",&cis);
	 fscanf(in,"%c",&trailer);
	 d_toe=clock_tow-toe;
	 if (d_toe>302400.0) d_toe=d_toe-604800.0;
	 else if (d_toe<-302400.0)d_toe=d_toe+604800.0;
//
//    If week is current and time is less than 2 hours old use for hot start
//    note: gps_week is computed from the PCs real time clock
//          and does not roll over at 1024
//
	 if (week==(gps_week % 1024) && fabs(d_toe)<7200.0)
    {
     gps_eph[id].valid=1;
	 gps_eph[id].health=health;
	 gps_eph[id].week=week;
	 gps_eph[id].ura=ura;
	 gps_eph[id].toe=toe;
	 gps_eph[id].iode=iode;
	 gps_eph[id].toc=toc;
	 gps_eph[id].iodc=iodc;
	 gps_eph[id].tgd=tgd;
	 gps_eph[id].af0=daf0;
	 gps_eph[id].af1=daf1;
	 gps_eph[id].af2=daf2;
	 gps_eph[id].ety=ety;
	 gps_eph[id].inc0=inc0;
	 gps_eph[id].idot=idot;
	 gps_eph[id].omegadot=omegadot;
	 gps_eph[id].sqra=esqra;
	 gps_eph[id].dn=dn;
	 gps_eph[id].w0=w0;
	 gps_eph[id].w=w;
	 gps_eph[id].ma=ma;
	 gps_eph[id].cuc=cuc;
	 gps_eph[id].cus=cus;
	 gps_eph[id].crc=crc;
	 gps_eph[id].crs=crs;
	 gps_eph[id].cic=cic;
	 gps_eph[id].cis=cis;
	 if (gps_eph[id].sqra >0.0) gps_eph[id].wm=19964981.84/pow(gps_eph[id].sqra,3);
    if (out_debug)  write_Debug_ephemeris(id);
    }
		 }
		 fclose(in);

	  }
}

/*******************************************************************************
FUNCTION write_almanac()
RETURNS  None.

PARAMETERS None.

PURPOSE
	This function writes the broadcast almanac data to a file for later
	use.  In particular to support a warm or hot start.

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

void write_almanac()
{
  int i;
  if (almanac_valid==1)
  {
  out=fopen("current.alm","w+");
  for (i=1;i<=32;i++)
  {
    if ( gps_alm[i].inc >0.0)
    {
		fprintf(out,"**** Week %4d almanac for PRN-%2d ***********\n",
					gps_alm[i].week%1024,i);
      fprintf(out,"ID:                         %3d\n",i);
      fprintf(out,"Health:                     %3d\n",gps_alm[i].health);
      fprintf(out,"Eccentricity:               %10.9e\n",gps_alm[i].ety);
      fprintf(out,"Time of Applicability(s):   %10.9e\n",gps_alm[i].toa);
      fprintf(out,"Orbital Inclination(rad):   %10.9e\n",gps_alm[i].inc);
      fprintf(out,"Rate of Right Ascen(R/s):   %10.9e\n",gps_alm[i].rra);
      fprintf(out,"SQRT(A) (m^1/2):            %10.9e\n",gps_alm[i].sqa);
      fprintf(out,"Right Ascen at TOA(rad):    %10.9e\n",gps_alm[i].lan);
      fprintf(out,"Argument of Perigee(rad):   %10.9e\n",gps_alm[i].aop);
      fprintf(out,"Mean Anom(rad):             %10.9e\n",gps_alm[i].ma);
      fprintf(out,"Af0(s):                     %10.9e\n",gps_alm[i].af0);
      fprintf(out,"Af1(s/s):                   %10.9e\n",gps_alm[i].af1);
		fprintf(out,"week:                       %4d   \n",gps_alm[i].week%1024);
      fprintf(out,"\n");
    }
  }
  fclose(out);
  }
}

/*******************************************************************************
FUNCTION write_ephemeris()
RETURNS  None.

PARAMETERS None.

PURPOSE
	This function writes the broadcast ephemeris data to a file for later
	use.  In particular to support a hot start.

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

void write_ephemeris()
{
  int i;
  out=fopen("current.eph","w+");
  for (i=1;i<=32;i++)
  {
    if ( gps_eph[i].inc0 >0.0)
    {
      fprintf(out,"**** Ephemeris for PRN-%2d ***********\n",i);
      fprintf(out,"ID:                         %3d\n",i);
      fprintf(out,"Health:                     %3d\n",gps_eph[i].health);
		fprintf(out,"Week:                       %4d\n",   gps_eph[i].week);
		fprintf(out,"URA:                        %3d\n",   gps_eph[i].ura);
		fprintf(out,"E Time of Applic(s):        %12.11e\n",gps_eph[i].toe);
		fprintf(out,"IODE:                       %4d\n",   gps_eph[i].iode);
		fprintf(out,"C Time of Applic(s):        %12.11e\n",gps_eph[i].toc);
		fprintf(out,"IODC:                       %4d\n",   gps_eph[i].iodc);
		fprintf(out,"Tgd(s):                     %12.11e\n",gps_eph[i].tgd);
		fprintf(out,"Af0(s):                     %12.11e\n",gps_eph[i].af0);
		fprintf(out,"Af1(s/s):                   %12.11e\n",gps_eph[i].af1);
		fprintf(out,"Af2(s/s/s):                 %12.11e\n",gps_eph[i].af2);
		fprintf(out,"Eccentricity:               %12.11e\n",gps_eph[i].ety);
		fprintf(out,"Orbital Inclination(rad):   %12.11e\n",gps_eph[i].inc0);
		fprintf(out,"inc rate (r/s)              %12.11e\n",gps_eph[i].idot);
		fprintf(out,"Rate of Right Ascen(R/s):   %12.11e\n",gps_eph[i].omegadot);
		fprintf(out,"SQRT(A) (m^1/2):            %12.11e\n",gps_eph[i].sqra);
		fprintf(out,"dn                          %12.11e\n",gps_eph[i].dn);
		fprintf(out,"Right Ascen at TOE(rad):    %12.11e\n",gps_eph[i].w0);
		fprintf(out,"Argument of Perigee(rad):   %12.11e\n",gps_eph[i].w);
		fprintf(out,"Mean Anom(rad):             %12.11e\n",gps_eph[i].ma);
		fprintf(out,"Cuc(rad):                   %12.11e\n",gps_eph[i].cuc);
		fprintf(out,"Cus(rad):                   %12.11e\n",gps_eph[i].cus);
		fprintf(out,"Crc(m):                     %12.11e\n",gps_eph[i].crc);
		fprintf(out,"Crs(m):                     %12.11e\n",gps_eph[i].crs);
		fprintf(out,"Cic(rad):                   %12.11e\n",gps_eph[i].cic);
		fprintf(out,"Cis(rad):                   %12.11e\n",gps_eph[i].cis);
      fprintf(out,"\n");
    }
  }
  fclose(out);
}
/*******************************************************************************
FUNCTION write_ephemeris()
RETURNS  None.

PARAMETERS None.

PURPOSE
	This function writes the broadcast ephemeris data to a file for later
	use.  In particular to support a hot start.

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

void write_Debug_ephemeris(int i)
{
		fprintf(debug,"**** Ephemeris for PRN-%2d ***********\n",i);
		fprintf(debug,"ID:                         %3d\n",i);
		fprintf(debug,"Health:                     %3d\n",gps_eph[i].health);
		fprintf(debug,"Week:                       %4d\n",   gps_eph[i].week);
		fprintf(debug,"URA:                        %3d\n",   gps_eph[i].ura);
		fprintf(debug,"E Time of Applic(s):        %12.11e\n",gps_eph[i].toe);
		fprintf(debug,"IODE:                       %4d\n",   gps_eph[i].iode);
		fprintf(debug,"C Time of Applic(s):        %12.11e\n",gps_eph[i].toc);
		fprintf(debug,"IODC:                       %4d\n",   gps_eph[i].iodc);
		fprintf(debug,"Tgd(s):                     %12.11e\n",gps_eph[i].tgd);
		fprintf(debug,"Af0(s):                     %12.11e\n",gps_eph[i].af0);
		fprintf(debug,"Af1(s/s):                   %12.11e\n",gps_eph[i].af1);
		fprintf(debug,"Af2(s/s/s):                 %12.11e\n",gps_eph[i].af2);
		fprintf(debug,"Eccentricity:               %12.11e\n",gps_eph[i].ety);
		fprintf(debug,"Orbital Inclination(rad):   %12.11e\n",gps_eph[i].inc0);
		fprintf(debug,"inc rate (r/s)              %12.11e\n",gps_eph[i].idot);
		fprintf(debug,"Rate of Right Ascen(R/s):   %12.11e\n",gps_eph[i].omegadot);
		fprintf(debug,"SQRT(A) (m^1/2):            %12.11e\n",gps_eph[i].sqra);
		fprintf(debug,"dn                          %12.11e\n",gps_eph[i].dn);
		fprintf(debug,"Right Ascen at TOE(rad):    %12.11e\n",gps_eph[i].w0);
		fprintf(debug,"Argument of Perigee(rad):   %12.11e\n",gps_eph[i].w);
		fprintf(debug,"Mean Anom(rad):             %12.11e\n",gps_eph[i].ma);
		fprintf(debug,"Cuc(rad):                   %12.11e\n",gps_eph[i].cuc);
		fprintf(debug,"Cus(rad):                   %12.11e\n",gps_eph[i].cus);
		fprintf(debug,"Crc(m):                     %12.11e\n",gps_eph[i].crc);
		fprintf(debug,"Crs(m):                     %12.11e\n",gps_eph[i].crs);
		fprintf(debug,"Cic(rad):                   %12.11e\n",gps_eph[i].cic);
		fprintf(debug,"Cis(rad):                   %12.11e\n",gps_eph[i].cis);
		fprintf(debug,"\n");

}


/*******************************************************************************
FUNCTION write_ion_utc()
RETURNS  None.

PARAMETERS None.

PURPOSE
	This function writes the broadcast ionospheric correction data and the
      parameters to tie GPS time to UTC to a file for later use.

WRITTEN BY
	Clifford Kelley

*******************************************************************************/

void write_ion_utc()
{
   out=fopen("ion_utc.dat","w+");
   fprintf(out,"al0:                        %e\n",al0);
   fprintf(out,"al1:                        %e\n",al1);
   fprintf(out,"al2:                        %e\n",al2);
   fprintf(out,"al3:                        %e\n",al3);
   fprintf(out,"b0:                         %f\n",b0);
   fprintf(out,"b1:                         %f\n",b1);
   fprintf(out,"b2:                         %f\n",b2);
   fprintf(out,"b3:                         %f\n",b3);
   fprintf(out,"a0                          %f\n",a0);
   fprintf(out,"a1                          %f\n",a1);
   fprintf(out,"dtls                        %f\n",dtls);
   fprin