gpsfuncs.cpp

OpenSource GPS is software for x86 PCs that allows you to acquire, track and demodulate signals from

	     pow(track_sat[i].y-y,2)+
	     pow(track_sat[i].z-z,2));
      ms[1][i]=(track_sat[i].x-x)/r;
      ms[2][i]=(track_sat[i].y-y)/r;
      ms[3][i]=(track_sat[i].z-z)/r;
      ms[4][i]=1.0;
  }
  for (i=1;i<=nsl;i++)
  {
     msx=ms[1][i]*north.x+ms[2][i]*north.y+ms[3][i]*north.z;
     msy=ms[1][i]*east.x+ms[2][i]*east.y;
     msz=ms[1][i]*up.x+ms[2][i]*up.y+ms[3][i]*up.z;
	  ms[1][i]=msx;ms[2][i]=msy;ms[3][i]=msz;ms[4][i]=1.0;
  }
  a1=0.;b1=0.;c1=0.;d1=0.;
  e1=0.;f1=0.;g1=0.;h1=0.;
  i1=0.;j1=0.;k1=0.;l1=0.;
  m1=0.;n1=0.;o1=0.;p1=0.;
  for (k=1;k<=nsl;k++)
  {
     a1+=ms[1][k]*ms[1][k];
     b1+=ms[1][k]*ms[2][k];
     c1+=ms[1][k]*ms[3][k];
     d1+=ms[1][k]*ms[4][k];
//   e1+=ms[2][k]*ms[1][k];
     f1+=ms[2][k]*ms[2][k];
     g1+=ms[2][k]*ms[3][k];
     h1+=ms[2][k]*ms[4][k];
//   i1+=ms[3][k]*ms[1][k];
//   j1+=ms[3][k]*ms[2][k];
     k1+=ms[3][k]*ms[3][k];
     l1+=ms[3][k]*ms[4][k];
//   m1+=ms[1][k];
//   n1+=ms[2][k];
//   o1+=ms[3][k];
     p1+=ms[4][k];
   }
   o1=l1;m1=d1;n1=h1;e1=b1;i1=c1;j1=g1;
//
//     SOLVE FOR THE DIAGONALS OF THE MATRIX INVERSE
//
	 denom=(k1*p1-l1*o1)*(a1*f1-b1*e1)+(l1*n1-j1*p1)*(a1*g1-c1*e1)+
			 (j1*o1-k1*n1)*(a1*h1-d1*e1)+(l1*m1-i1*p1)*(c1*f1-b1*g1)+
			 (i1*o1-k1*m1)*(d1*f1-b1*h1)+(i1*n1-j1*m1)*(c1*h1-d1*g1);
	 ddx=(f1*(k1*p1-l1*o1)+g1*(l1*n1-j1*p1)+h1*(j1*o1-k1*n1))/denom;// ddx=ndop^2
	 ddy=(a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1))/denom;// ddy=edop^2
	 ddz=(a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1))/denom;// ddz=vdop^2
	 ddt=(a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1))/denom;// ddt=tdop^2
    if ( denom<=0.0 )
    {
		 hdop=1.e6;      // something went wrong
		 vdop=1.e6;      // set dops to a large number
		 tdop=1.e6;
		 pdop=1.e6;
		 gdop=1.e6;
    }
    else
    {
		 hdop=sqrt(ddx+ddy);          // rss of ndop and edop
		 vdop=sqrt(ddz);
		 tdop=sqrt(ddt);
		 pdop=sqrt(ddx+ddy+ddz);      // rss of ndop, edop, and vdop
		 gdop=sqrt(ddx+ddy+ddz+ddt);  // rss of ndop, edop, vdop, and tdop
	 }
//	gotoxy(1,24);
//	printf("dops->");

}
#endif

#ifdef DJGPP
void  dops( int nsl)
{
  double ddx,ddy,ddz,ddt,r,rxy,ms[5][13],x,y,z;
  double a1,b1,c1,d1,e1,f1,g1,h1,i1,j1,k1,l1,m1,n1,o1,p1,denom;
  double msx,msy,msz;
  int i,k;
  ecef north,east,up;
//	gotoxy(1,24);
//	printf("->dops");
  x=rec_pos_xyz.x;
  y=rec_pos_xyz.y;
  z=rec_pos_xyz.z;
  r=sqrt(x*x+y*y+z*z);
  rxy=sqrt(x*x+y*y);
  north.x=-x/rxy*z/r;
  north.y=-y/rxy*z/r;
  north.z= rxy/r;
  east.x=-y/rxy;
  east.y= x/rxy;
//east.z=0.0; just for completeness
  up.x=x/r;
  up.y=y/r;
  up.z=z/r;
  for (i=1;i<=nsl;i++)
  {
//
//   Compute line of sight vectors
//
      r=sqrt(pow(track_sat[i].x-x,2)+
	     pow(track_sat[i].y-y,2)+
	     pow(track_sat[i].z-z,2));
      ms[1][i]=(track_sat[i].x-x)/r;
      ms[2][i]=(track_sat[i].y-y)/r;
      ms[3][i]=(track_sat[i].z-z)/r;
      ms[4][i]=1.0;
  }
  for (i=1;i<=nsl;i++)
  {
     msx=ms[1][i]*north.x+ms[2][i]*north.y+ms[3][i]*north.z;
     msy=ms[1][i]*east.x+ms[2][i]*east.y;
     msz=ms[1][i]*up.x+ms[2][i]*up.y+ms[3][i]*up.z;
	  ms[1][i]=msx;ms[2][i]=msy;ms[3][i]=msz;ms[4][i]=1.0;
  }
  a1=0.;b1=0.;c1=0.;d1=0.;
  e1=0.;f1=0.;g1=0.;h1=0.;
  i1=0.;j1=0.;k1=0.;l1=0.;
  m1=0.;n1=0.;o1=0.;p1=0.;
  for (k=1;k<=nsl;k++)
  {
     a1+=ms[1][k]*ms[1][k];
     b1+=ms[1][k]*ms[2][k];
     c1+=ms[1][k]*ms[3][k];
     d1+=ms[1][k]*ms[4][k];
//   e1+=ms[2][k]*ms[1][k];
     f1+=ms[2][k]*ms[2][k];
     g1+=ms[2][k]*ms[3][k];
     h1+=ms[2][k]*ms[4][k];
//   i1+=ms[3][k]*ms[1][k];
//   j1+=ms[3][k]*ms[2][k];
     k1+=ms[3][k]*ms[3][k];
     l1+=ms[3][k]*ms[4][k];
//   m1+=ms[1][k];
//   n1+=ms[2][k];
//   o1+=ms[3][k];
     p1+=ms[4][k];
   }
   o1=l1;m1=d1;n1=h1;e1=b1;i1=c1;j1=g1;
//
//     SOLVE FOR THE DIAGONALS OF THE MATRIX INVERSE
//
	 denom=(k1*p1-l1*o1)*(a1*f1-b1*e1)+(l1*n1-j1*p1)*(a1*g1-c1*e1)+
			 (j1*o1-k1*n1)*(a1*h1-d1*e1)+(l1*m1-i1*p1)*(c1*f1-b1*g1)+
			 (i1*o1-k1*m1)*(d1*f1-b1*h1)+(i1*n1-j1*m1)*(c1*h1-d1*g1);
	 ddx=(f1*(k1*p1-l1*o1)+g1*(l1*n1-j1*p1)+h1*(j1*o1-k1*n1))/denom;// ddx=ndop^2
	 ddy=(a1*(k1*p1-l1*o1)+c1*(l1*m1-i1*p1)+d1*(i1*o1-k1*m1))/denom;// ddy=edop^2
	 ddz=(a1*(f1*p1-h1*n1)+b1*(h1*m1-e1*p1)+d1*(e1*n1-f1*m1))/denom;// ddz=vdop^2
	 ddt=(a1*(f1*k1-g1*j1)+b1*(g1*i1-e1*k1)+c1*(e1*j1-f1*i1))/denom;// ddt=tdop^2
    if ( denom<=0.0 )
    {
		 hdop=1.e6;      // something went wrong
		 vdop=1.e6;      // set dops to a large number
		 tdop=1.e6;
		 pdop=1.e6;
		 gdop=1.e6;
    }
    else
    {
		 hdop=sqrt(ddx+ddy);          // rss of ndop and edop
		 vdop=sqrt(ddz);
		 tdop=sqrt(ddt);
		 pdop=sqrt(ddx+ddy+ddz);      // rss of ndop, edop, and vdop
		 gdop=sqrt(ddx+ddy+ddz+ddt);  // rss of ndop, edop, vdop, and tdop
	 }
//	gotoxy(1,24);
//	printf("dops->");

}
#endif

/*******************************************************************************
FUNCTION tropo_iono(float az, float el, double gps_time)
RETURNS  signal time delay due to troposphere and ionosphere (single frequency)

PARAMETERS
			az         float
			el         float
			gps_time   double

PURPOSE
	This function corrects the pseudoranges with a tropospheric model
	and the broadcast ionospheric message corrections.

WRITTEN BY
	Clifford Kelley

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

double tropo_iono(char ch,float az,float el,double gps_time)
{
  double d_Trop,alt_factor;
  double d_Ion,psi,phi,lambdai,phim,per,x,F,amp,t;

  //  Try a simple troposphere model
//	gotoxy(1,24);
//	printf("->tropo_iono");
  if (current_loc.hae>200000.0) alt_factor=0.0;
  else if (current_loc.hae<0.0) alt_factor=1.0;
  else alt_factor=exp(-current_loc.hae*1.33e-4);
  if (m_tropo==1) d_Trop=2.47/(sin(el)+.0121)*alt_factor/c;
  else d_Trop=0.0;
//  if (out_kalman==1) fprintf(kalm,",az= %12.6f, el=%12.6f, tropo= %12.10lf",
//                             az,el,d_Trop);
  chan[ch].Tropo=d_Trop;
//  if (el<(mask_angle-0.035))
//  {
//  		printf("ch=%d, el=%lf",ch,el*57.296);
//  		fprintf(kalm,"ch=%d, el=%lf",ch,el);
//  }
  //  Use an ionosphere model

  if (m_iono==1)
  {
	 psi=0.0137/(el/pi+0.11)-.022;
	 phi=current_loc.lat/pi+psi*cos(az);
	 if      (phi > 0.416) phi= 0.416;
	 else if (phi <-0.416 )phi=-0.416;
	 lambdai=current_loc.lon/pi+psi*sin(az)/cos(phi*pi);
	 t=43200.0*lambdai+gps_time-int((43200.0*lambdai+gps_time)/86400.)*86400.;
	 if (t<0.0) t=t+86400.0;
	 phim=phi+0.064*cos((lambdai-1.617)*pi);
//
//  If available from the nav message use its Ionosphere model
//
	 if (b0 != 0.0 && al0 != 0.0)
	 {
		per=b0+b1*phim+b2*phim*phim+b3*phim*phim*phim;
		amp=al0+al1*phim+al2*phim*phim+al3*phim*phim*phim;
	 }
//
// else try this set of default iono model parameters
//
	 else
	 {
		per=141312.0-32768.0*phim-131072.0*phim*phim-65536.0*phim*phim*phim;
		amp=3.46e-8+7.45e-9*phim-1.19e-7*phim*phim+5.96e-8*phim*phim*phim;
	 }
	 if ( per <72000.0 ) per=72000.0;
	 x=2.*pi*(t-50400.)/per;
	 F=1.0+16.0*pow(0.53-el/pi,3);
	 if ( amp < 0.0 ) amp=0.0;
	 if (fabs(x) < 1.5707)  d_Ion=F*(5.0e-9+amp*(1.0-x*x/2.+x*x*x*x/24.0));
	 else                   d_Ion=F*5.0e-9;
  }
  else d_Ion=0.0;
  chan[ch].Iono=d_Ion;
//	gotoxy(1,24);
//	printf("tropo_iono->");
  return(d_Trop+d_Ion);
}

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

PARAMETERS None.

PURPOSE
	This function reads the broadcast ionospheric correction model and the
	gps time to UTC conversion parameters from "ion_utc.dat" which is
	created by the program when the data has been read from the satellites

WRITTEN BY
	Clifford Kelley

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

void  read_ion_utc(void)
{
   char text[27];
   if ((in = fopen("ion_utc.dat", "rt")) == NULL)
   {
       printf("Cannot open ion_utc.dat file.\n");
   }
   else
   {
	handle=fileno(in);
	while (!feof(in))
	{
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al0);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al1);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al2);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&al3);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b0);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b1);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b2);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&b3);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&a0);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&a1);
	fscanf(in,"%27c",&text);
	fscanf(in,"%f",&dtls);
	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"