📄 gpsfuncs.cpp
字号:
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;
float d_toe;
if ((in = fopen("current.eph", "rt")) == NULL)
{
printf("Cannot open currrent.eph file.\n");
}
else
{
handle=fileno(in);
while (!eof(handle))
{
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);
}
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;
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_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);
fprintf(out,"tot %f\n",tot);
fprintf(out,"WNt %f\n",WNt);
fprintf(out,"WNlsf %f\n",WNlsf);
fprintf(out,"DN %f\n",DN);
fprintf(out,"dtlsf %f\n",dtlsf);
fclose(out);
}
/*******************************************************************************
FUNCTION rss(long a, long b)
RETURNS long integer
PARAMETERS
a long integer
b long integer
PURPOSE
This function finds the fixed point magnitude of a 2 dimensional vector
WRITTEN BY
Clifford Kelley
*******************************************************************************/
inline long rss(long a,long b )
{
long result,c,d;
c=labs(a);
d=labs(b);
if (c==0 && d==0) result=0;
else
{
if (c>d) result=(d>>1)+c;
else result=(c>>1)+d;
}
return (result);
}
/*******************************************************************************
FUNCTION fix_sqrt(long x)
RETURNS long integer
PARAMETERS
x long integer
PURPOSE
This function finds the fixed point square root of a long integer
WRITTEN BY
Clifford Kelley
*******************************************************************************/
long fix_sqrt(long x)
{
long xt,scr;
int i;
i=0;
xt=x;
do
{
xt=xt>>1;
i++;
} while (xt>0);
i=(i>>1)+1;
xt=x>>i;
do
{
scr=xt*xt;
scr=x-scr;
scr=scr>>1;
scr=scr/xt;
xt=scr+xt;
} while (scr!=0);
xt=xt<<7;
return(xt);
}
/*******************************************************************************
FUNCTION fix_atan2(long y,long x)
RETURNS long integer
PARAMETERS
x long in-phase fixed point value
y long quadrature fixed point value
PURPOSE
This function computes the fixed point arctangent represented by
x and y in the parameter list
1 radian = 16384
based on the power series f-f^3*2/9
WRITTEN BY
Clifford Kelley
Fixed for y==x added special code for x==0 suggested by Joel Barnes, UNSW
*******************************************************************************/
#define SCALED_PI_ON_2 25736L
#define SCALED_PI 51472L
inline long fix_atan2(long y,long x)
{
long result,n,n3;
if ((x==0) && (y==0))
return(0); // invalid case
if (x>0 && x>=labs(y))
{
n=(y<<14)/x;
n3=((((n*n)>>14)*n)>>13)/9;
result=n-n3;
}
else if (x<=0 && -x>=labs(y))
{
n=(y<<14)/x;
n3=((((n*n)>>14)*n)>>13)/9;
if ( y>0) result=n-n3+SCALED_PI;
else if (y<=0) result=n-n3-SCALED_PI;
}
else if (y>0 && y>labs(x))
{
n=(x<<14)/y;
n3=((((n*n)>>14)*n)>>13)/9;
result=SCALED_PI_ON_2-n+n3;
}
else if (y<0 && -y>labs(x))
{
n=(x<<14)/y;
n3=((((n*n)>>14)*n)>>13)/9;
result=-n+n3-SCALED_PI_ON_2;
}
return(result);
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -