getsp3.cpp

卫星单点定位程序

#include "GetSP3.h"

SP3 * GetSP3(char *FileName)
{
	int		i, j;
	long	line	= 0;		//数据文件的行号，第一行为 0 
	long	fpos,fpos0;			//文件位置指针
	char 	str[_MAX_SP3_DATA];	//包含一行数据的字符串
	SP3		*sp3;
	FILE	*datafile;
	long	num	= 0;			//已读取的历元数
	/*char	markname[7][3] = 	//标签名称
	{
		"##",
		"+ ",
		"++",
		"%c",
		"%f",
		"%i",
		"/*",
	}*/

	sp3	= (SP3*) malloc ( sizeof (SP3));
	if (sp3 == NULL)	{
		printf ("内存分配错误！");
		return NULL;
	}
	memset (sp3, 0, sizeof (SP3));			//结构中的所有都置 0

	datafile	= fopen (FileName, "rb");
	if (datafile == NULL)	{
		printf ("轨道数据文件不存在，请检查文件是否存在！");
		free (sp3);
		return NULL;
	}
	
	//读取文件头
	for (line=0; line<22; line++)	{
		fgets(str, _MAX_SP3_DATA, datafile);
		switch (line) {
		case 0:
			strncpy(sp3->hdr.ver_id, str, 2);				//版本标识符
			strncpy(sp3->hdr.type_id, str + 2, 1);			//位置或速度标识
			csatoCommonTime(str + 3, &sp3->hdr.firt_epoch);	//轨道数据首历元
			sp3->hdr.epoch_count	= csatol(str + 32, 7);
			strncpy(sp3->hdr.data_type, str + 40, 5);
			strncpy(sp3->hdr.ref_frame, str + 46, 5);
			strncpy(sp3->hdr.orbit_type, str + 52, 3);
			strncpy(sp3->hdr.agency, str + 56, 4);
			break;
		case 1:
			sp3->hdr.wn			= csatol(str + 3, 4);
			sp3->hdr.gps_seconds= csatof(str + 8, 15);
			sp3->hdr.interval	= csatol(str + 24, 14);
			break;
		//读取 PRN 列表
		case 2:
			sp3->hdr.sat_num	= (unsigned char)csatol(str + 4, 2);
			for (i=0; i<4; i++)	{
				if (i>0)	{
					line++;
					fgets(str, _MAX_SP3_DATA, datafile); 
				}
				for (j=0; j<17; j++)	{
					sp3->hdr.prn[i * 17 + j]
						= (unsigned char)csatol(str + 9 + 3 * j, 3);
				}
			}
			break;
		//读取精度列表
		case 7:
			for (i=0; i<4; i++)	{
				if (i>0)	{
					line++;
					fgets(str, _MAX_SP3_DATA, datafile); 
				}
				for (j=0; j<17; j++)	{
					sp3->hdr.precision[i * 17 + j]	
						= (unsigned char)csatol(str + 9 + 3 * j, 3);
				}
			}
			break;
/*		case :
			break;
		case :
			break;
		case :
			break;*/
		}
	}

	//读取轨道数据
	sp3->rec	= (SP3REC *)malloc(sizeof(SP3REC) * (sp3->hdr.epoch_count));
	memset(sp3->rec, 0, sizeof (SP3REC) * (sp3->hdr.epoch_count));
	fpos0	= ftell (datafile);	//取得读取前的文件指针位置
	for ( ; ; )	{
		fpos	= GetSP3Rec(datafile, fpos0, sp3->hdr, &sp3->rec[num]);
		if (fpos == -2)	{
			return sp3;
		}
		if (fpos == -1)	{
			continue;
		}
		if (fpos > 0)	{
			num++;
			fpos0	= fpos;
		}
	}	
	return NULL;
}


//将一行包含通用时的字符串转换成 一个通用时数据
//返回值：正常返回 0 ，否则返回 1
int csatoCommonTime(const char *str,	//包含时间数据的字符串
					COMMONTIME *ct)		//转换出的时间数据
{
	ct->year	= (unsigned short) csatol(str, 4);
	ct->month	= (unsigned char) csatol(str + 5, 2);
	ct->day		= (unsigned char) csatol(str + 8, 2);
	ct->hour	= (unsigned char) csatol(str + 11, 2);
	ct->minute	= (unsigned char) csatol(str + 14, 2);
	ct->second	= csatof(str + 17, 11);
	return 0;
}


//从包含记录的字符串中提取记录
int GetSP3Rec(FILE		*datafile,	//包含时间数据的字符串
			  long		fpos,		//文件位置
			  SP3HDR	hdr,		//SP3 格式文件的文件头数据
			  SP3REC	*rec)		//用于保存数据的记录结构
{
	char			str[_MAX_SP3_DATA]	= {0};	//保存数据字符串
	int				i	= 0;					//卫星序号
	unsigned char	prn	= 0;					//卫星 PRN 号
	int				tmr	= 0;					//历元时刻是否已读，1 为已读
	long			fpos0	= fpos;				//前一个文件位置
	long aaa = sizeof(SP3RECDATA) * hdr.sat_num;

	rec->data	= (SP3RECDATA *) malloc(aaa);
	memset(rec->data, 0, sizeof (SP3RECDATA) * hdr.sat_num);
	fseek (datafile, fpos, SEEK_SET);
	for ( ; ; )	{
		fgets (str, _MAX_SP3_DATA, datafile);

		//读取历元时刻
		if ((str[0] == '*') && (str[1] == ' ') && (tmr == 0))	{
			csatoCommonTime(str + 3, &rec->tm);
			tmr	= 1;
			continue;
		}
		//读取位置参数和钟差参数
		if ((str[0] == 'P') && (tmr == 1))	{
			prn = (unsigned char)csatol(str + 1, 3);
			for (i=0; i<hdr.sat_num; i++)	{
				if (prn == hdr.prn[i])	{
					rec->data[i].prn		= prn;
					rec->data[i].position.x	= csatof(str + 4, 14);
					rec->data[i].position.y	= csatof(str + 18, 14);
					rec->data[i].position.z	= csatof(str + 32, 14);
					rec->data[i].clk_bias	= csatof(str + 46, 14);
					break;
				}
			}
		}
		//读取速度参数和钟飘参数
/*		if (str[0] == 'V')	{
			prn = (unsigned char)csatol(str + 1, 3);
			for (i=0; i<hdr.sat_num; i++)	{
				if (prn == hdr.prn[i])	{
					rec->data[i].velocity.x	= csatof(str + 4, 14);
					rec->data[i].velocity.y	= csatof(str + 18, 14);
					rec->data[i].velocity.z	= csatof(str + 32, 14);
					rec->data[i].clk_rate	= csatof(str + 46, 14);
					break;
				}
			}
			
		}*/
		//读取到文件结束标识
		if (csfeof (str) == 1)	{
			if (tmr == 0)	{
				free(rec->data);
				return -2;
			}
			else {
				return fpos0;
			}
		}
		//读到下一个历元时刻
		if ((str[0] =='*') && (tmr == 1))	{
			return fpos0;
		}
		fpos0	= ftell(datafile);
	}
	return -1;
}


//判断字符串是否为文件结束标识符 "EOF"
int csfeof(const char *str)
{
	if ((str[0] == 'E') && (str[1] == 'O') && (str[2] == 'F'))
		return 1;
	else 
		return 0;
}


//拉格朗日插值公式
double lagrange(double x[], double y[], double x0, int m)
{
	int i, j;
	double y0 = 0;
	double tmp;
	for (i = 0; i < m; i++)	{
		tmp = 1;
		for (j = 0; j < m; j++)	{
			if (j != i)	{
				tmp *= (x0 - x[j]) / (x[i] - x[j]);
			}
		}
		y0 += y[i] * tmp; 
	}	
	return y0;
}


//获取指定卫星在指定历元时刻在ECEF下的坐标和钟差。
int GetOrbNClkSP3 (SP3* sp3,					//sp3：指向SP3数据的指针；
				long nPRN,					//卫星的PRN号
				PCOMMONTIME ctEpoch,		//历元时刻；
				PCRDCARTESIAN pcrdOrb,		//指向卫星在ECEF下坐标的指针；
				double* pdSVClkBias)		//指向卫星钟差的指针。
{
	int i;
	double coeft[18];					//进行插值运算的的历元的GPS周内时间，0 ~ n
	double coef[4][18];				//要插值项目的每个历元的值
	GPSTIME gt;						//给定历元的GPS时
	long fgt;						//给定历元的前一个sp3记录的GPS时的周内秒
	long fgt_No;					//fgt 的历元序号
	long PRN_No;					//nPRN在sp3数据的PRN列表里的序号

	CommonTimeToGPSTime(ctEpoch, &gt);
	fgt = long(gt.tow.sn - sp3->hdr.gps_seconds) / 900 * 900;
	fgt_No = fgt / 900;
	for (i = 0; i < sp3->hdr.sat_num; i++)	{
		if (nPRN == sp3->hdr.prn[i])	{
			PRN_No = i;
		}
	}

	//如果给定历元恰好有相应的SP3记录
	if ((fgt == gt.tow.sn) && (gt.tow.tos < 10e-11))	{		
		pcrdOrb->x = sp3->rec[fgt_No].data[PRN_No].position.x * 1000;
		pcrdOrb->y = sp3->rec[fgt_No].data[PRN_No].position.y * 1000;
		pcrdOrb->z = sp3->rec[fgt_No].data[PRN_No].position.z * 1000;
		*pdSVClkBias = sp3->rec[fgt_No].data[PRN_No].clk_bias * 0.000001;
		return 0;
	}

	//如果给定历元没有相应的SP3记录，则通过拉格朗日插值求出
	for (i = 0; i < 18; i++)	{
		coeft[i] = fgt + (i - 8) * 900;
		coef[0][i] = sp3->rec[fgt_No - 8 + i].data[PRN_No].position.x * 1000;
		coef[1][i] = sp3->rec[fgt_No - 8 + i].data[PRN_No].position.y * 1000;
		coef[2][i] = sp3->rec[fgt_No - 8 + i].data[PRN_No].position.z * 1000;
		coef[3][i] = sp3->rec[fgt_No - 8 + i].data[PRN_No].clk_bias * 0.000001;
	}
	pcrdOrb->x = lagrange(coeft, coef[0], gt.tow.sn + gt.tow.tos - sp3->hdr.gps_seconds, 18);
	pcrdOrb->y = lagrange(coeft, coef[1], gt.tow.sn + gt.tow.tos - sp3->hdr.gps_seconds, 18);
	pcrdOrb->z = lagrange(coeft, coef[2], gt.tow.sn + gt.tow.tos- sp3->hdr.gps_seconds, 18);
	*pdSVClkBias = lagrange(coeft, coef[3], gt.tow.sn + gt.tow.tos - sp3->hdr.gps_seconds, 18);
	return 0;
}