sim_util.c

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

	fgets(buffer,BUFSIZE,fpTraj);
	sscanf(buffer,"%s  %s %s ", bufstr1, bufstr2, bufstr3);
	if ( strcmpi(strupr(bufstr3),"KINEMATIC") != 0)
		return FAILURE;

	//Skip four lines, and goto staring time
	for (i = 0; i<4 ; i++)
		fgets(buffer,BUFSIZE,fpTraj);

	// Get the simulation beginning time
	fgets(buffer,BUFSIZE,fpTraj);
	sscanf(buffer,"%lf ", &dtemp);
	if ( dtemp < 0 || dtemp > 604800 )
	{
		printf("Starting time is wrong. \n");
		return FAILURE;
	}
	else
		Start_time = dtemp;

	// Skip three lines
	for (i = 0; i<3 ; i++)
		fgets(buffer,BUFSIZE,fpTraj);

	// Get the begnning position (X, Y, X) in WGS84 frame
	fgets(buffer,BUFSIZE,fpTraj);
	sscanf(buffer,"%lf , %lf , %lf ", &enu_origin[0], &enu_origin[1], &enu_origin[2] );

	for (i = 0; i<3 ; i++)
		fgets(buffer,BUFSIZE,fpTraj);

	// Get the initial velocity in local level frame
	fgets(buffer,BUFSIZE,fpTraj);
	sscanf(buffer,"%lf , %lf , %lf ", &ini_vel[0], &ini_vel[1], &ini_vel[2] );
	ini_vel[2] = 0;		// Remove vertical movement

	for (i = 0; i<11 ; i++)
		fgets(buffer,BUFSIZE,fpTraj);

	// Start to explain movement
	for ( ; ;)
	{
		fgets(buffer,BUFSIZE,fpTraj);
		fgets(buffer,BUFSIZE,fpTraj);

		if (buffer[0] == 'L' || buffer[0] == 'l')
			ProcessLinear(buffer, &Start_time, ini_pos, ini_vel, &Rx_Pos_Index );
		else if (buffer[0] == 'C' || buffer[0] == 'c')
			ProcessCircular(buffer, &Start_time, ini_pos, ini_vel, &Rx_Pos_Index);
		else
			break;
	}
	// Save in  tempory file.
	if (Rx_Pos_Index == 0)
		return FAILURE;

	if (Start_time > RxPos[Rx_Pos_Index-1][0] )
	{
		RxPos[Rx_Pos_Index][0]	=	Start_time;
		RxPos[Rx_Pos_Index][1]	=	ini_pos[0];
		RxPos[Rx_Pos_Index][2]	=	ini_pos[1];
		RxPos[Rx_Pos_Index][3]	=	ini_pos[2];
		RxPos[Rx_Pos_Index][4]	=	ini_vel[0];
		RxPos[Rx_Pos_Index][5]	=	ini_vel[1];
		RxPos[Rx_Pos_Index][6]	=	ini_vel[2];
		Rx_Pos_Index ++;
	}

	*pRx_Num	=	Rx_Pos_Index;

#if ENUPOSFILE
	// Save the true user location (E, N, U) in local level frame in position file  
	{
		FILE *fpos;
		long int icount;

		fpos = fopen("traj.dat","w");
		for (icount=0; icount < Rx_Pos_Index; icount ++)
		{
			fprintf(fpos, "%lf %lf %lf %lf %lf %lf %lf\n", RxPos[icount][0], RxPos[icount][1],
				RxPos[icount][2], RxPos[icount][3], RxPos[icount][4], RxPos[icount][5], RxPos[icount][6] );
		}
		fclose (fpos);
	}
#endif


	{
		long int icount;
		double pos_enu[3], vel_enu[3];
		for (icount=0; icount < Rx_Pos_Index; icount ++)
		{
			pos_enu[0]	=	RxPos[icount][1];
			pos_enu[1]	=	RxPos[icount][2];
			pos_enu[2]	=	RxPos[icount][3];
			vel_enu[0]	=	RxPos[icount][4];
			vel_enu[1]	=	RxPos[icount][5];
			vel_enu[2]	=	RxPos[icount][6];

			enu2xyz(pos_enu, enu_origin, &RxPos[icount][1]);
			enu2xyz(vel_enu, enu_origin, &RxPos[icount][4]);
			RxPos[icount][4]	-=	enu_origin[0];
			RxPos[icount][5]	-=	enu_origin[1];
			RxPos[icount][6]	-=	enu_origin[2];
		}
	}

#if XYZPOSFILE
	// Save the true user location (X, Y, Z) in WGS84 in position file  
	{
		FILE *fpos;
		long int icount;

		fpos = fopen("traj.dat","w");
		for (icount=0; icount < Rx_Pos_Index; icount ++)
		{
			fprintf(fpos, "%lf %lf %lf %lf %lf %lf %lf\n", RxPos[icount][0], RxPos[icount][1],
				RxPos[icount][2], RxPos[icount][3], RxPos[icount][4], RxPos[icount][5], RxPos[icount][6] );
		}
		fclose (fpos);
	}
#endif

	return SUCCESS;
}


//******************************************************************************
//**	Function:	ProcessLinear										
//**	Input:		Command describing constant acceleration movement 
//**	Tasks:		Analyse constant acceleration mevement, generating trajectory information
//**	Output:		User postion at each computation point
//**					postion and velocity information for the next movement
//******************************************************************************
int		ProcessLinear(char *buffer, double *Start_time, double *ini_pos, double *ini_vel, long int *pIndex)
{
	double	time_len;
	double	current_time, last_comp_time, stop_time;
	double  st_time;
	double	delta_t, delta_t_2;
	double  delta_e, delta_n, delta_u;
	double  acc_e, acc_n, acc_u;
	double	vel_e,  vel_n, vel_u ;
	long int	Index;

	st_time	=	*Start_time;
	Index = (*pIndex);
	if (Index == 0)
	{
		current_time = floor(st_time/TrajTD)*TrajTD;
		last_comp_time = current_time - TrajTD;
	}
	else
	{
		last_comp_time = RxPos[Index-1][0];
	}


	// read time length of the movement together with the constant acceleration in E, N directions
	//		in local level frame
	sscanf(strchr(buffer,',')+1, "%lf, %lf, %lf, %lf ", &time_len, &acc_e, &acc_n, &acc_u );
	acc_u = 0;	// Remove vertcal movement

	stop_time = st_time + time_len;
	for ( current_time = last_comp_time + TrajTD ; current_time < stop_time; current_time += TrajTD )
	{
		// Calculate user postion and velocity with respect to time
		current_time = floor(current_time*100000+0.5)/100000;
		delta_t		=	current_time - st_time;
		delta_t_2	=	delta_t*delta_t/2;
		delta_e		=	ini_vel[0] * delta_t +  acc_e * delta_t_2;
		delta_n		=	ini_vel[1] * delta_t +  acc_n * delta_t_2;
		delta_u		=	ini_vel[2] * delta_t +  acc_u * delta_t_2;
		vel_e		=	ini_vel[0] + acc_e*delta_t;
		vel_n		=	ini_vel[1] + acc_n*delta_t;
		vel_u		=	ini_vel[2] + acc_u*delta_t;

		RxPos[Index][0] = current_time;
		RxPos[Index][1]	=	ini_pos[0] + delta_e;
		RxPos[Index][2]	=	ini_pos[1] + delta_n;
		RxPos[Index][3]	=	ini_pos[2] + delta_u;
		RxPos[Index][4]	=	vel_e;
		RxPos[Index][5]	=	vel_n;
		RxPos[Index][6]	=	vel_u;
//  Acce
		RxPos[Index][7]	=	acc_e;
		RxPos[Index][8]	=	acc_n;
		RxPos[Index][9]	=	acc_u;


		Index ++;
	}

	delta_t		=	stop_time - st_time;
	delta_t_2	=	delta_t*delta_t/2;
	ini_pos[0]	+=	ini_vel[0] * delta_t +  acc_e * delta_t_2;
	ini_pos[1]	+=	ini_vel[1] * delta_t +  acc_n * delta_t_2;
	ini_pos[2]	+=	ini_vel[2] * delta_t +  acc_u * delta_t_2;
	ini_vel[0]	+=	acc_e*delta_t;
	ini_vel[1]	+=	acc_n*delta_t;
	ini_vel[2]	+=	acc_u*delta_t;
	*Start_time =	stop_time;
	*pIndex		=	Index;		// Next Index
	return 0;
}


//******************************************************************************
//**	Function:	ProcessCircular										
//**	Input:		Command describing constant velocity rate circular movement 
//**	Tasks:		Analyse constant velocity rate circular mevement, 
//**						generating trajectory information
//**	Output:		User postion at each computation point
//**					postion and velocity information for the next movement
//******************************************************************************
int		ProcessCircular(char *buffer, double *Start_time, double *ini_pos, double *ini_vel, long int *pIndex)
{
	double	time_len;
	double	current_time, last_comp_time, stop_time;
	double  radius;
	double  st_time;
	double	delta_t;
	double	temp_e, temp_n, vel_e,vel_n, delta_e, delta_n;
	double  origin_e, origin_n;
	double	omega, rotation_angle, delta_ang;
	long int	Index;
	double  TrajTD1, dtemp;
	char 	bufstr1[BUFSIZE];		// input buffer
	char *buf1, *buf2, *buf3;
	double	temp_1, temp_2;

	TrajTD1	=	TrajTD;

	st_time	=	*Start_time;
	Index = (*pIndex);
	if (Index == 0)
	{
		dtemp = st_time/TrajTD;
		dtemp = ceil(dtemp);
		current_time = dtemp*TrajTD;
		last_comp_time = current_time - TrajTD;
	}
	else
	{
		last_comp_time = RxPos[Index-1][0];
	}

	// Analyse the command to get 
	//			time length of the circular movement
	//			rotating direction:   clockwise(Right) / anticlock wise (Left)
	buf1 = strchr(buffer,',')+1;
	buf2 = strchr(buf1,',')+1;
	buf3 = strchr(buf2,',')+1;

	sscanf(buf1, "%lf",&time_len );
	sscanf(buf2, "%s", bufstr1);
	sscanf(buf3, "%lf",&radius );

	stop_time = st_time + time_len;

	// Determine the origin of the circular
	if ( sqrt(ini_vel[0]*ini_vel[0] + ini_vel[1] * ini_vel[1]) < 1e-10)
	{
		temp_e = 0;
		temp_n = 0;
	}
	else
	{
		temp_e = (ini_vel[0]/sqrt(ini_vel[0]*ini_vel[0] + ini_vel[1] * ini_vel[1])) * radius;
		temp_n = (ini_vel[1]/sqrt(ini_vel[0]*ini_vel[0] + ini_vel[1] * ini_vel[1])) * radius;
	}
	
	if ( strchr(bufstr1, 'R') || strchr(bufstr1, 'r') )
		rotation_angle = PI/2;
	else
		rotation_angle = -PI/2;

	temp_1 = cos(rotation_angle)*temp_e + sin(rotation_angle)*temp_n ;
	temp_2 = -sin(rotation_angle)*temp_e + cos(rotation_angle)*temp_n ;

	origin_e = ini_pos[0] + temp_1;
	origin_n = ini_pos[1] + temp_2;

	temp_e = - temp_1;
	temp_n = - temp_2;

	omega = sqrt(ini_vel[0]*ini_vel[0] + ini_vel[1] * ini_vel[1])/radius;


	// Calculate user postion and velocity with respect to time
	for ( current_time = last_comp_time + TrajTD ; current_time < stop_time; current_time += TrajTD )
	{

		current_time = floor(current_time*100000+0.5)/100000;
		delta_t		=	current_time - st_time;
		delta_ang	=	omega * delta_t;

		if ( strchr(bufstr1, 'R') || strchr(bufstr1, 'r') )
			rotation_angle = delta_ang;
		else
			rotation_angle = -delta_ang;


		delta_e = cos(rotation_angle)*temp_e + sin(rotation_angle)*temp_n ;
		delta_n = -sin(rotation_angle)*temp_e + cos(rotation_angle)*temp_n ;

		vel_e	= cos(rotation_angle)*ini_vel[0] + sin(rotation_angle)*ini_vel[1] ;
		vel_n = -sin(rotation_angle)*ini_vel[1] + cos(rotation_angle)*ini_vel[1] ;

		
		RxPos[Index][0] =	current_time;
		RxPos[Index][1]	=	origin_e + delta_e;
		RxPos[Index][2]	=	origin_n + delta_n;
		RxPos[Index][3]	=	ini_pos[2];
		RxPos[Index][4]	=	vel_e;
		RxPos[Index][5]	=	vel_n;
		RxPos[Index][6]	=	ini_vel[2];
//  Acce
//		RxPos[Index][7]	=	acc_e;
//		RxPos[Index][8]	=	acc_n;
//		RxPos[Index][9]	=	acc_u;


		Index ++;
	}

	delta_t		=	stop_time - st_time;
	delta_ang	=	omega * delta_t;

	if ( strchr(bufstr1, 'R') || strchr(bufstr1, 'r') )
		rotation_angle = delta_ang;
	else
		rotation_angle = -delta_ang;

	delta_e = cos(rotation_angle)*temp_e + sin(rotation_angle)*temp_n ;
	delta_n = -sin(rotation_angle)*temp_e + cos(rotation_angle)*temp_n ;
	vel_e	= cos(rotation_angle)*ini_vel[0] + sin(rotation_angle)*ini_vel[1] ;
	vel_n = -sin(rotation_angle)*ini_vel[1] + cos(rotation_angle)*ini_vel[1] ;

	ini_pos[0]  =	origin_e + delta_e;
	ini_pos[1]  =	origin_n + delta_n;
	ini_vel[0]	=	vel_e;
	ini_vel[1]	=	vel_n;
	*Start_time =	stop_time;
	*pIndex		=	Index;		// Next Index	

	return 0;
}


void complex_sum (double re1,double im1, double re2, double im2, double *pre, double *pim) 
{
	*pre = re1 + re2;
	*pim = im1 + im2;

	return;
}

void complex_sub (double re1,double im1, double re2, double im2, double *pre, double *pim) 
{
	*pre = re1 - re2;
	*pim = im1 - im2;

	return;
}


void complex_multiply (double re1,double im1, double re2, double im2, double *pre, double *pim) 
{
	*pre = re1*re2 - im1*im2;
	*pim = re1*im2 + re2*im1;

	return;
}

void complex_div (double re1,double im1, double re2, double im2, double *pre, double *pim) 
{
	double tmp;
	tmp = re2*re2+im2*im2;
	
	complex_multiply (re1, im1, re2, -im2, pre, pim);

	*pre /= tmp;
	*pim /= tmp;

	return;
}

void ReadIonoPara(OPTION* pOption)
{
	FILE* fp;
	char fileline[100];
	char option[21];
	fp=fopen(pOption->Eph_filename,"r");
	if(fp==NULL) 
	{
		perror("Cannot find ephemeris file!");
		exit(0);
	}
	option[20]='\0';

	// default values
	pOption->alpha[0]= 0.1770E-07;
	pOption->alpha[1]=-0.7451E-08; 
	pOption->alpha[2]=-0.5960E-07;
	pOption->alpha[3]= 0.1192E-06;
	pOption->beta[0]=0.1311E+06; 
	pOption->beta[1]=-0.1311E+06;
	pOption->beta[2]=0.6554E+05;
	pOption->beta[3]=-0.2621E+06;

	while(!feof(fp))
	{
		fgets(fileline,100,fp);
		memcpy(option,fileline+60,sizeof(char)*20);
		if(strcmp(option,"ION ALPHA           ")==0)
		{
			fileline[10]=fileline[22]=fileline[34]=fileline[46]='e';
			sscanf(fileline,"%lf %lf %lf %lf",&(pOption->alpha[0]),&(pOption->alpha[1]),
				&(pOption->alpha[2]),&(pOption->alpha[3]));
		}
		else if(strcmp(option,"ION BETA            ")==0)
		{
			fileline[10]=fileline[22]=fileline[34]=fileline[46]='e';
			sscanf(fileline,"%lf %lf %lf %lf",&(pOption->beta[0]),&(pOption->beta[1]),
				&(pOption->beta[2]),&(pOption->beta[3]));
		}
		else if(strcmp(option,"END OF HEADER       ")==0)
		{
			break;
		}
	}
	fclose(fp);
}