getrnxnav.cpp

卫星单点定位程序

#include "GetRnxNav.h"

void* GetGMN (char *FileName)
{
	int		i;
	char	markname[7][21] = 	//标签名称
	{ 
		"RINEX VERSION / TYPE",
		"PGM / RUN BY / DATE",
		"ION ALPHA",
		"ION BETA",
		"DELTA-UTC: A0,A1,T,W",
		"LEAP SECONDS",
		"END OF HEADER"
	};
	char	mark_number[_MAX_RNXN_MARK_NUMBER];	//数据字符串
	char	mark_name[_MAX_RNXN_MARK_NAME];		//文件头标签
	long	fpos,fpos0;							//文件位置指针
	PGMN	gmn;
	FILE	*datafile;
	int		nr1	= 100;							//已分配内存的 历元 数
	gmn	= (PGMN) malloc ( sizeof (GMN));
	if (gmn == NULL)	{
		printf ("内存分配错误！");
		return NULL;
	}
	memset (gmn, 0, sizeof (GMN));			//结构中的所有都置 0

	datafile	= fopen (FileName, "r");
	if (datafile == NULL)	{
		printf ("导航数据文件不存在，请检查文件是否存在！");
		free (gmn);
		return NULL;
	}
	
	for ( ; ; )	{
		int tmp1=0;        //数据类型标志序号
		fgets (mark_number, _MAX_RNXN_MARK_NUMBER, datafile);		//读入数据字符串
		if (strlen(mark_number) < 60)	{
			continue;
		}
		fgets (mark_name, _MAX_RNXN_MARK_NAME, datafile);			//读入标签字符串
		for (i=0; i<7; i++)	{
			tmp1	= strncmp(mark_name, markname[i], strlen(markname[i]));	//逐个比较标签
			if (tmp1!=0) {
				continue;
			}
			tmp1	= i;

			switch (tmp1)	{
				case 0:				//RINEX VERSION / TYPE
					sscanf (mark_number, "%d.%d", &gmn->hdr.ver_major, 
						&gmn->hdr.ver_minor);	
					break;

				case 1:				//PGM / RUN BY / DATE
//					strncpy (gmn->hdr.Pgm, mark_number, 20);
//					strncpy (gmn->hdr.RunBy, mark_number+20, 20);
//					strncpy (gmn->hdr.Date, mark_number+40, 20);	
					break;

				case 2:				//ION ALPHA
					gmn->hdr.ion_alpha[3]	= csatof(mark_number + 38, 12);
					gmn->hdr.ion_alpha[2]	= csatof(mark_number + 26, 12);
					gmn->hdr.ion_alpha[1]	= csatof(mark_number + 14, 12);
					gmn->hdr.ion_alpha[0]	= csatof(mark_number + 2, 12);
					break;

				case 3:				//ION BETA
					gmn->hdr.ion_beta[3]	= csatof(mark_number + 38, 12);
					gmn->hdr.ion_beta[2]	= csatof(mark_number + 26, 12);
					gmn->hdr.ion_beta[1]	= csatof(mark_number + 14, 12);
					gmn->hdr.ion_beta[0]	= csatof(mark_number + 2, 12);
					break;

				case 4: 			//DELTA-UTC: A0,A1,T,W
					gmn->hdr.delta_utc.W	= csatol(mark_number + 50, 9);
					gmn->hdr.delta_utc.T	= csatol(mark_number + 41, 9);
					gmn->hdr.delta_utc.A1	= csatof(mark_number + 22, 19);
					gmn->hdr.delta_utc.A0	= csatof(mark_number + 3, 19);
					break;

				case 5: 			//LEAP SECONDS
					gmn->hdr.leap_seconds	= csatol(mark_number,6);
					break;
			}
			break;
		}
		if (tmp1==6)	break;		//如果读到文件头结束标签则结束读取文件头
	}
	
	gmn->record	= (PGMNREC) malloc (nr1 * sizeof (GMNREC));
	memset (gmn->record, 0, nr1 * sizeof (GMNREC));

	for ( ; ; )	{
		fpos0	= ftell (datafile);	//取得读取前的文件指针位置
		fpos	= GetGMNRecord (datafile, &gmn->record[gmn->hdr.rec_num], fpos0);
		if (fpos == -2)	{
			break;
		}
		if (fpos == -1)	{
			fseek (datafile, fpos0, SEEK_SET);
		}
		if (fpos >= 0)	{
			gmn->hdr.rec_num++;
			if (gmn->hdr.rec_num == nr1)	{
				PGMNREC	rec;
				nr1	+= 100;
				rec	= (PGMNREC) malloc (nr1 * sizeof (GMNREC));
				memset (rec, 0, nr1 * sizeof (GMNREC));
				memcpy (rec,gmn->record, (nr1-100) * sizeof (GMNREC));
				free (gmn->record);
				gmn->record	= rec;
			}
			fpos0	= fpos;
		}
	}
	return gmn;
}


//读取一个记录
long GetGMNRecord (FILE *datafile,	//包含导航电文的文件指针
				  PGMNREC rec,		//纪录结构指针
				  long fpos)		//文件位置
{
	char	str[_MAX_RNXN_DATA]	= {0};
	int		i=0;
	
	fseek (datafile, fpos, SEEK_SET);
	if (feof(datafile) != 0)	{
		return -2;
	}
	for ( ; ; )	{
		fgets (str, _MAX_RNXN_DATA, datafile);
		if (i < 7 && feof (datafile) != 0)	{
			return -2;
		}
		if (strlen(str) < 20)	{	//如果字符串长度小于20则读取下一行
			continue;
		}
		if	(i == 0)	{	//包含PRN和时间参数
			sscanf(str, "%d%d%d%d%d%d",
				&rec->PRN, &rec->TOC.year, &rec->TOC.month, &rec->TOC.day,
				&rec->TOC.hour, &rec->TOC.minute);
			rec->ClkDriftRate	= csatof(str + 60, 19);
			rec->ClkDrift		= csatof(str + 41, 19);
			rec->ClkBias		= csatof(str + 22, 19);
			rec->TOC.second		= csatof(str + 17, 5);

			if (rec->TOC.year > 79)	{
				rec->TOC.year+=1900;
			}
			else	{
				rec->TOC.year+=2000;
			}
			i++;
			continue;
		}
		//其他数据行
		switch (i)	{
			case 1:
				rec->M0			= csatof(str + 60, 19);
				rec->DeltaN		= csatof(str + 41, 19);
				rec->Crs		= csatof(str + 22, 19);
				rec->IODE		= csatof(str + 3, 19);
				break;
			case 2:
				rec->SqrtA		= csatof(str + 60, 19);
				rec->Cus		= csatof(str + 41, 19);
				rec->e			= csatof(str + 22, 19);
				rec->Cuc		= csatof(str + 3, 19);
				break;
			case 3:
				rec->Cis		= csatof(str + 60, 19);
				rec->Omega		= csatof(str + 41, 19);
				rec->Cic		= csatof(str + 22, 19);
				rec->TOE		= csatof(str + 3, 19);
				break;
			case 4:
				rec->OmegaDot	= csatof(str + 60, 19);
				rec->omega		= csatof(str + 41, 19);
				rec->Crc		= csatof(str + 22, 19);
				rec->i0			= csatof(str + 3, 19);
				break;
			case 5:
				rec->L2PDataFlag		= csatof(str + 60, 19);
				rec->GPSWeek			= csatof(str + 41, 19);
				rec->CodesOnL2Channel	= csatof(str + 22, 19);
				rec->iDot				= csatof(str + 3, 19);
				break;
			case 6:
				rec->IODC		= csatof(str + 60, 19);
				rec->TGD		= csatof(str + 41, 19);
				rec->SVHealth	= csatof(str + 22, 19);
				rec->SVAccuracy	= csatof(str + 3, 19);
				break;
			case 7:
				rec->Sparel3		= csatof(str + 60, 19);
				rec->Sparel2		= csatof(str + 41, 19);
				rec->Sparel1		= csatof(str + 22, 19);
				rec->TransTimeOfMsg	= csatof(str + 3, 19);
				break;
		}
		i++;
		if	(i == 8)
			return ftell (datafile);
	}
	return -1;
}


//计算与给定的历元时刻最接近的有轨道参数的历元时刻，只计算小时，分秒为随机数。
int GetNearEpoch (PCOMMONTIME NearEpoch, PCOMMONTIME Epoch, int normal)
{
	JULIANDAY jd;
	memcpy(NearEpoch, Epoch, sizeof (COMMONTIME));
	CommonTimeToJulianDay(NearEpoch, &jd);
	if (normal == 1)	{
		if (Epoch->hour == 23)	{
			jd.tod.sn += 3600;
			JulianDayToCommonTime(&jd, NearEpoch);
			return 0;
		}
		else	{
			NearEpoch->hour	+= NearEpoch->hour % 2;
			return 0;
		}
	}
	else	{
		if (Epoch->hour == 0)	{
			jd.day--;
			JulianDayToCommonTime(&jd, NearEpoch);
			NearEpoch->hour	= 23;
			return 0;
		}
		else	{
			NearEpoch->hour	= NearEpoch->hour + NearEpoch->hour % 2 - 1;
			return 0;
		}
	}
	return 1;
}


//根据给定的历元和 PRN 确定记录的序号
long GetRecSN  (PGMN GMN, long PRN, PCOMMONTIME Epoch)
{
	long i;
	COMMONTIME NearEpoch;
	GetNearEpoch(&NearEpoch, Epoch, 1);
	for (i=0; i<GMN->hdr.rec_num; i++)	{
		if (   (GMN->record[i].PRN == PRN)
			&& (GMN->record[i].TOC.year == NearEpoch.year)
			&& (GMN->record[i].TOC.month == NearEpoch.month)
			&& (GMN->record[i].TOC.day == NearEpoch.day)
			&& (GMN->record[i].TOC.hour == NearEpoch.hour))	{
			return i;
		}
	}
	GetNearEpoch(&NearEpoch, Epoch, 0);
	for (i=0; i<GMN->hdr.rec_num; i++)	{
		if (   (GMN->record[i].PRN == PRN)
			&& (GMN->record[i].TOC.year == NearEpoch.year)
			&& (GMN->record[i].TOC.month == NearEpoch.month)
			&& (GMN->record[i].TOC.day == NearEpoch.day)
			&& (GMN->record[i].TOC.hour == NearEpoch.hour))	{
			return i;
		}
	}
	return -1;
}


//获取指定卫星在指定历元时刻在ECEF下的坐标和钟差。
int GetOrbNClk (PGMN pGMN, long nPRN,	PCOMMONTIME ctEpoch,
				 PCRDCARTESIAN pcrdOrb, double* pdSVClkBias)
{
	long	sn;		//与指定历元最接近的历元的记录的序号
	GPSTIME gtEpoch;// ctEpoch 的 GPS 时表示法
	GPSTIME TOC;	// TOC 的 GPS 时表示法
	double	n;		//运动角速度
	double	M;		//平近点角
	double	E;		//偏近点角
	double	E0;		//偏近点角的近似值
	double	vk;		//真近点角
	double	phik;	//升交角距
	double	delteuk;//升交角距的改正数
	double	delterk;//向径的改正数
	double	delteik;//轨道倾角改正数
	double	u;		//经过改正的升交角距
	double	r;		//经过改正的向径
	double	i;		//经过改正的轨道倾角
	double	x, y;	//卫星在轨道平面上的位置
	double	Omega;	//改正后的升交点经度
	double	tk;		//

	//查找所需的记录
	sn	= GetRecSN  (pGMN, nPRN, ctEpoch);

    //开始计算卫星坐标
	CommonTimeToGPSTime(ctEpoch, &gtEpoch);
	tk	= gtEpoch.tow.sn + gtEpoch.tow.tos - pGMN->record[sn].TOE;
	if (tk > 302400)	{
		tk -= 604800;
	}
	if (tk < -302400)	{
		tk += 604800;
	}
	n	= sqrt(MIU) / pow(pGMN->record[sn].SqrtA, 3) 
		+ pGMN->record[sn].DeltaN;
	M	= pGMN->record[sn].M0 + n * tk;
	for (E0=E=M; ; ){
		E0	= E;
		E	= M + pGMN->record[sn].e * sin(E0);
		if (fabs(E - E0) < 10e-12 )
			break;
	}
	vk	= atan2(sqrt(1 - pGMN->record[sn].e * pGMN->record[sn].e) * sin(E),
				cos(E) - pGMN->record[sn].e);
	phik = pGMN->record[sn].omega + vk;
	delteuk = pGMN->record[sn].Cuc * cos(2 * phik)
		    + pGMN->record[sn].Cus * sin(2 * phik);
	delterk = pGMN->record[sn].Crc * cos(2 * phik) 
			+ pGMN->record[sn].Crs * sin(2 * phik);
	delteik = pGMN->record[sn].Cic * cos(2 * phik)
			+ pGMN->record[sn].Cis * sin(2 * phik);
	
	u	= phik + delteuk;
	r	= pow(pGMN->record[sn].SqrtA, 2) * (1 - pGMN->record[sn].e * cos(E))
		+ delterk;
	i	= pGMN->record[sn].i0 + delteik + pGMN->record[sn].iDot * tk;
	x	= r * cos(u);
	y	= r * sin(u);
	Omega= pGMN->record[sn].Omega + pGMN->record[sn].OmegaDot * tk
		 - ROTATION_SPEED_OF_EARTH * (gtEpoch.tow.sn + gtEpoch.tow.tos);

	pcrdOrb->x	= x * cos(Omega) - y * cos(i) * sin(Omega);
	pcrdOrb->y	= x * sin(Omega) + y * cos(i) * cos(Omega);
	pcrdOrb->z	= y * sin(i);

	//卫星钟差
	double F, dt;
	F	= -2 * sqrt(MIU) / (c * c);
	dt	= F * pGMN->record[sn].e * pGMN->record[sn].SqrtA * sin(E);
	CommonTimeToGPSTime(&pGMN->record[sn].TOC, &TOC);
	tk	= gtEpoch.tow.sn + gtEpoch.tow.tos - TOC.tow.sn - TOC.tow.tos;
	*pdSVClkBias = pGMN->record[sn].ClkBias + pGMN->record[sn].ClkDrift * tk
		+ pGMN->record[sn].ClkDriftRate * tk * tk + dt -pGMN->record[sn].TGD;
	return 0;
}