gensamples.c

GPS卫星导航接收机的仿真程序。用C语言实现

		while (1)
		{
			//mm2=mm2+1;
			//compute sat position at Trx-Tp in the ECEF frame 1
			comp_SV_posvel(prn, &xs1,&ys1,&zs1,&vxs1,&vys1,&vzs1,&clk_bias,time_Rx[m1]-Tp2);

			//rotate sat position in frame 1 to frame 2 at time Trx-Tp
			rotate(&xs2,&ys2,&zs2,xs1,ys1,zs1,Tp2);

			//Compute new range and Tp2 in frame 2
			Tp22 = sqrt((xs2-xu)*(xs2-xu)+(ys2-yu)*(ys2-yu)+(zs2-zu)*(zs2-zu))/LightSpeed;

			Diff_Tp = fabs(LightSpeed*(Tp22-Tp2));
			if( Diff_Tp < 0.000001)
				break;
			Tp2 = Tp22;
		}
		
		Tp_tmp[m1] = Tp2 - clk_bias + dTrop[m1-start_count]/LightSpeed+dIono[m1-start_count];
		Tc_tmp[m1] = Tp2 - clk_bias + dTrop[m1-start_count]/LightSpeed-dIono[m1-start_count];

		r  = sqrt((xs2-xu)*(xs2-xu) + (ys2-yu)*(ys2-yu) + (zs2-zu)*(zs2-zu));
		
	}
		

	//Interpolation
	//Compute second derivative of the boundary condition
	spline(&(time_Rx[start_count])-1, &(Tp_tmp[start_count])-1, (end_count - start_count + 1 ), 1e30, 1e30, y2_Tp-1);
	spline(&(time_Rx[start_count])-1, &(Tc_tmp[start_count])-1, (end_count - start_count + 1 ), 1e30, 1e30, y2_Tc-1);		
	
	for (m1=0;m1<n_T;m1++)
	{
		splint(&time_Rx[start_count], &Tp_tmp[start_count], y2_Tp, (end_count - start_count + 1 ), time[m1]+StartTime, &Tp[m1]);
		splint(&time_Rx[start_count], &Tc_tmp[start_count], y2_Tc, (end_count - start_count + 1 ), time[m1]+StartTime, &Tc[m1]);
	}

//	splintarray(&time_Rx[start_count], &Tp_tmp[start_count], y2_Tp, (end_count - start_count + 1 ), time, StartTime, Tp,n_T);
//	splintarray(&time_Rx[start_count], &Tc_tmp[start_count], y2_Tc, (end_count - start_count + 1 ), time, StartTime, Tc,n_T);
	if(dTrop!=NULL) 
	{
		free(dTrop);
		dTrop = NULL;
	}
		
}


//******************************************************************************
//**	Function:	comp_eph												
//**	Tasks:		Read ephemeris from IGS broadcast ephemeris file (RINEX)
//******************************************************************************
void comp_eph(	int prn_sv, OPTION *pOption, double t	)		
{
	
	// variables definition
	FILE   *fp_eph;
	char   line1[100],line2[100],line3[100],line4[100],line5[100],line6[100],line7[100];
	char   line[100];
	int	   c1,c2,c3,c4,c5,c6;
	double c7,c8, c9, c10, Toe_n,time_n;
	char   *pdest;
	char   string[] = "END OF HEADER";	
	double	time_diff = 1000000.0;

	time_n = t;

	memset((void *) &eph, 0, sizeof(eph));
		
	// printf("Computer Ephemeris. \r\n");
	//open ephemeris file
	if( (fp_eph  = fopen( pOption->Eph_filename , "r" )) == NULL )
	{
		printf("Cannot Open Source File !! \n");
		exit(0);
	}
		
	//printf("please wait...\n");

	////////////////////////////////////////////////////////////////
	//read header
	while (!feof(fp_eph))
	{
		fgets( line, 100, fp_eph );
		pdest=strstr(line,"END OF HEADER");
		if (pdest!=0)
			break;					
	}

	////////////////////////////////////////////////////////////////
	//read navigation data,check prn first
	while (!feof(fp_eph))
	{
		fgets( line, 100, fp_eph);
		line[37] = 'e';line[56] = 'e';line[75] = 'e';
		Read_line1( line,  &c1,&c2,&c3,&c4,&c5,&c6,&c7,&c8,&c9,&c10 );
				
		//Check if it is prn
		if( c1 == prn_sv ) //find prn
		{   
			fgets( line1, 100, fp_eph);
			fgets( line2, 100, fp_eph);
			fgets( line3, 100, fp_eph);
			fgets( line4, 100, fp_eph);
			fgets( line5, 100, fp_eph);
			fgets( line6, 100, fp_eph);
			fgets( line7, 100, fp_eph);
					
			line3[18] = 'e';line3[37] = 'e';line3[56] = 'e';line3[75] = 'e';
			Read_line( line3,  &Toe_n,&c7,&c8,&c9 );

			if ( fabs(Toe_n - time_n) < time_diff )
			{
				time_diff	=	fabs(Toe_n - time_n);

				//read ephemeris data
				//line[37] = 'e';line[56] = 'e';line[75] = 'e';
				Read_line1( line,&c1,&c2,&c3,&c4,&c5,&c6,&c7,&eph.af0,&eph.af1,&eph.af2 );

				line1[18] = 'e';line1[37] = 'e';line1[56] = 'e';line1[75] = 'e';
				Read_line (line1, &eph.IODE, &eph.Crs,&eph.dn,&eph.M0 );
				//sscanf( line11, "%lf %lf %lf %lf", &c1,&eph.Crs,&eph.dn,&eph.M0 );

				line2[18] = 'e';line2[37] = 'e';line2[56] = 'e';line2[75] = 'e';
				Read_line( line2, &eph.Cuc,&eph.ec,&eph.Cus,&eph.sqrtA );

				line3[18] = 'e';line3[37] = 'e';line3[56] = 'e';line3[75] = 'e';
				Read_line( line3,  &eph.Toe,&eph.Cic,&eph.W0,&eph.Cis );

				line4[18] = 'e';line4[37] = 'e';line4[56] = 'e';line4[75] = 'e';
				Read_line( line4,  &eph.i0,&eph.Crc,&eph.omeg,&eph.omegdot );

				line5[18] = 'e';line5[37] = 'e';line5[56] = 'e';line5[75] = 'e';
				Read_line( line5,  &eph.Idot,&eph.codeL2,&eph.weekn,&eph.L2p );

				line6[18] = 'e';line6[37] = 'e';line6[56] = 'e';line6[75] = 'e';
				Read_line( line6,  &eph.SVaccuracy,&eph.SVhealth,&eph.Tgd,&eph.IODC );

				line7[18] = 'e';line7[37] = 'e';line7[56] = 'e';line7[75] = 'e';
				Read_line( line7,  &eph.Ttr,&eph.fitintv,&c9,&c10 );

				eph.Toc = eph.Toe;
			}
		}
	}			
	fclose (fp_eph);
}

//******************************************************************************
//**	Function:	comp_SV_posvel												
//**	Tasks:		Compute satellite postion and velocity
//******************************************************************************
void comp_SV_posvel(int prn, double *xs, double *ys, double *zs, double *vxs, double *vys, double *vzs, double *clk_bias, double t)
{
	double dt = 0.01,xs11=0.0, ys11=0.0, zs11=0.0, xs22=0.0, ys22=0.0, zs22=0.0, clk_off, clk;

	comp_sat_pos(prn, &xs11, &ys11, &zs11, &clk_off, t);
	//printf("xs11 ys11 zs11=%f,%f,%f,%f\r\n",t,xs11,ys11,zs11);

    comp_sat_pos(prn, &xs22, &ys22, &zs22, &clk, t+dt);
	//printf("xs22 ys22 zs22=%f,%f,%f,%f\r\n",t+dt,xs22,ys22,zs22);
	*xs  = xs11;
	*ys  = ys11;
	*zs  = zs11;
	*vxs = (xs22-xs11)/dt;
	*vys = (ys22-ys11)/dt;
	*vzs = (zs22-zs11)/dt;
	*clk_bias = clk_off; 
	//free(&xs11);free(&ys11);free(&zs11);free(&xs22);free(&ys22);free(&zs22);
	//printf("posvel=%f,%f,%f,%f,%f,%f\r\n",xs,ys,zs,vxs,vys,vzs);

}


//******************************************************************************
//**	Function:	comp_sat_pos												
//**	Tasks:		Compute satellite postion
//******************************************************************************
void comp_sat_pos(int prn, double *xsv, double *ysv, double *zsv, double *clk_offset, double t)
{
	double mu    = 3.986004418e14;   // WGS-84 value of the Earth's universal gravitation constant GM (meters^3/second^2)
	double f     = -4.442809305e-10, relative;
	double A,n0,n,T,M,E_temp,E_error,E,sin_nu,cos_nu,nu,phi,du,dr,di,u,r,i,Xhat,Yhat,Wc;

	A       = ephsv[prn].sqrtA * ephsv[prn].sqrtA;                               // Semi-major axis 
	n0      = sqrt(mu/(A*A*A));                             //Computed mean motion 
	n       = n0 + ephsv[prn].dn;                             //Corrected mean motion 

	T       = t - ephsv[prn].Toe;                                 //Time from ephemeris reference epoch (See: Note.1) 
	
	if (T > 302400)
		T = T - 604800;
	else
		if(T < -302400)
		T = T + 604800;
	
	M       = ephsv[prn].M0 + n*T;                                  //Mean anomaly 
	E_temp  = M;
	E_error = 1;
	while (E_error > 1e-12)                              //Given Mean-anomaly iterate for Eccentricity
	{
		E       = M + ephsv[prn].ec*sin(E_temp);                     //Kepler's equation for Eccentric Anomaly 
		E_error = fabs(E - E_temp);
		E_temp  = E;
	}

	sin_nu  = sqrt(1 - ephsv[prn].ec*ephsv[prn].ec)*sin(E) / (1 - ephsv[prn].ec*cos(E));     //sine of true anomaly 
	cos_nu  = (cos(E) - ephsv[prn].ec) / (1 - ephsv[prn].ec*cos(E));             //cosine of true anomaly  
	nu      = atan2(sin_nu,cos_nu);                       //True anomaly  
	phi     = nu + ephsv[prn].omeg;                                    //Argument of latitude 
	du      = ephsv[prn].Cus*sin(2*phi) + ephsv[prn].Cuc*cos(2*phi);           //Argument of latitude correction 
	dr      = ephsv[prn].Crs*sin(2*phi) + ephsv[prn].Crc*cos(2*phi);           //Radius correction 
	di      = ephsv[prn].Cis*sin(2*phi) + ephsv[prn].Cic*cos(2*phi);           //Correction to inclination 
	u       = phi + du;                                  //Corrected argument of latitude 
	r       = A*(1 - ephsv[prn].ec*cos(E)) + dr;                     //Corrected radius 
	i       = ephsv[prn].i0 + di + ephsv[prn].Idot*T;                          //Corrected inclination 
	Xhat    = r*cos(u);                                  //Position x in orbital plane 
	Yhat    = r*sin(u);                                  //Position y in orbital plane 
	Wc      = ephsv[prn].W0 + (ephsv[prn].omegdot - Wedot)*T - Wedot*ephsv[prn].Toe;         //Corrected longitude of ascending node 
	*xsv    = (Xhat*cos(Wc) - Yhat*cos(i)*sin(Wc));        //ECEF x 
	*ysv    = (Xhat*sin(Wc) + Yhat*cos(i)*cos(Wc));        //ECEF y 
	*zsv    = (Yhat*sin(i));                               //ECEF z 
	//printf("pos=%f,%f,%f,%f\r\n",t,*xsv,*ysv,*zsv);

	relative = f*ephsv[prn].ec*ephsv[prn].sqrtA*sin(E);

	//ephsv[prn]->af0=0;ephsv[prn]->af1=0;ephsv[prn]->af2=0;

	*clk_offset = ephsv[prn].af0 + ephsv[prn].af1*T + ephsv[prn].af2*T*T + relative-ephsv[prn].Tgd;

	//printf("T=%f     relative=%16.12f      clk_drift=%16.12f\r\n",T,relative,ephsv[prn].af1);


	//*clk_drift = ephsv[prn].af1 + 2.0*ephsv[prn].af2*T;		
}


void rotate(double *xs2, double *ys2, double *zs2, double xs1, double ys1, double zs1, double dt)
{
	*xs2 = xs1*cos(Wedot*dt)+ys1*sin(Wedot*dt);
	*ys2 = -xs1*sin(Wedot*dt)+ys1*cos(Wedot*dt);
	*zs2 = zs1;
}



//******************************************************************************
//**	Function:	spline												
//**	Tasks:		Second order derivative derivation for interpolation
//******************************************************************************
void spline(double x[], double y[], int n, double yp1, double ypn, double y2[])
{
	int i,k;
	double p,qn,sig,un,*u;
	
	u = vector(1,n-1);

	if (yp1 > 0.99e30)
		y2[1]=u[1]=0.0;
		//y2[0]=u[1]=0.0;
	else 
	{
		y2[1] = -0.5;
		u[1]=(3.0/(x[2]-x[1]))*((y[2]-y[1])/(x[2]-x[1])-yp1);
		//y2[0] = -0.5;
		//u[1]=(3.0/(x[1]-x[0]))*((y[1]-y[0])/(x[1]-x[0])-yp1);
	}

	for (i=2;i<=n-1;i++) 
	//for (i=1;i<=n-2;i++) 
	{ 
		sig=(x[i]-x[i-1])/(x[i+1]-x[i-1]);
		p=sig*y2[i-1]+2.0;
		y2[i]=(sig-1.0)/p;
		u[i]=(y[i+1]-y[i])/(x[i+1]-x[i]) - (y[i]-y[i-1])/(x[i]-x[i-1]);
		u[i]=(6.0*u[i]/(x[i+1]-x[i-1])-sig*u[i-1])/p;
	}

	if (ypn > 0.99e30)
		qn=un=0.0;
	else
	{
		qn=0.5;
		un=(3.0/(x[n]-x[n-1]))*(ypn-(y[n]-y[n-1])/(x[n]-x[n-1]));
		//un=(3.0/(x[n-1]-x[n-2]))*(ypn-(y[n-1]-y[n-2])/(x[n-1]-x[n-2]));
	}

	y2[n]=(un-qn*u[n-1])/(qn*y2[n-1]+1.0);
	//y2[n-1]=(un-qn*u[n-1])/(qn*y2[n-2]+1.0);
	for (k=n-1;k>=1;k--)
		y2[k]=y2[k]*y2[k+1]+u[k];
		//y2[k-1]=y2[k-1]*y2[k]+u[k];
	
	free_vector(u,1,n-1);
}


//******************************************************************************
//**	Function:	splint												
//**	Tasks:		interpolation
//******************************************************************************
void splint(double xa[], double ya[], double y2a[], int n, double x, double *y)
{
	void nrerror(char error_text[]);
	int klo,khi,k;
	double h,b,a;

	
	//klo=1;
	//khi=n;
	klo=0;
	khi=n-1;
	while (khi-klo > 1) 
	{
		k=(khi+klo) >> 1;
		if (xa[k] > x) khi=k;
		else klo=k;
	}
	
	h=xa[khi]-xa[klo];
	if (h == 0.0) 
		nrerror("Bad xa input to routine splint"); 
	a=(xa[khi]-x)/h;
	b=(x-xa[klo])/h;
	*y=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*(h*h)/6.0;
}

//******************************************************************************
//**	Function:	splint												
//**	Tasks:		interpolation
//******************************************************************************
void splintarray(double xa[], double ya[], double y2a[], int n, double x[], double startvalue, double y[],  int m)
{
	void nrerror(char error_text[]);
	int klo,khi;
	double h,b,a;
	double h2over6;
	int i;
	double xtemp;

	h=xa[1]-xa[0];
	if (h == 0.0) 
		nrerror("Bad xa input to routine splint"); 
	h2over6=(h*h)/6.0;
	for (i=0; i<m; i++)
	{
		xtemp=x[i]+startvalue;
		klo=(int)((xtemp-xa[0])/h);
		if(klo<0)
			klo=0;
		khi=klo+1;
		if(khi>=n) 
		{
			khi=n-1;
			klo=khi-1;
		}
		a=(xa[khi]-xtemp)/h;
		b=(xtemp-xa[klo])/h;
		y[i]=a*ya[klo]+b*ya[khi]+((a*a*a-a)*y2a[klo]+(b*b*b-b)*y2a[khi])*h2over6;
	}
}

/* nrerror() := Numerical Recipes standard error handler */
void nrerror(char *error_text)
{
	fprintf(stderr, "Numerical Recipes run-time error...\n");
	fprintf(stderr, "%s\n", error_text);
	fprintf(stderr, "...now exiting to system...\n");
	exit(1);
}

/* *vector() := Allocates a double vector with range [nl..nh] */
double *vector(int nl, int nh)
{
	double *v;

	v = (double *) malloc( (unsigned)(nh-nl+1) * sizeof(double) );
	if (!v)
		nrerror("\nallocation failure in vector()");
	return v-nl;
}

void free_vector(double *v, int nl, int nh)
{
	free( (char *)(v + nl) );