gpsnavmsg.cpp

根据GPS卫星观测文件（.00O）与星历文件(.00N)

#include<string>
#include<cmath>
#include<fstream>
#include<iostream>
#include <algorithm>
#include  "Time.h" 
#include  "GPSNAVMSG.h"
#include  "func.h"

using namespace std;
 
GPSNAVMSGHDR& GPSNAVMSGHDR::ReadNavMsgHeader(ifstream& in)
{
		char sTemp[255];
		string sCheck;
		char sDataTemp[20];
	
		in.getline(sTemp,200,'\n');  //version,what is major version?

		in.getline(sTemp,200);

		sCheck.assign(&sTemp[60],&sTemp[60+3]);
	 
		if(sCheck == "PGM") {
			strMid(szPgm,sTemp,0,20);
 	        strMid(szRunBy,sTemp,20,20);
		   	strMid(szDate, sTemp,40,20);
		}
				
		in.getline(sTemp,200);
		sCheck.assign(&sTemp[60],&sTemp[60+7]);
		if(sCheck == "COMMENT") {

			while( sCheck == "COMMENT" ){
				in.getline(sTemp,200);
				sCheck.assign(&sTemp[60],&sTemp[60+7]);
			}			
		}
		
			sCheck.assign(&sTemp[67],&sTemp[67+6]);  //用于判断是否为文件头结尾

//			if(sCheck == "HEADER") 
//						cout<<"I read the header just now!\n";
			if(sCheck != "HEADER")
			{
				sCheck.assign(&sTemp[60],&sTemp[60+9]);
				if(sCheck == "ION ALPHA")
				{
					{
					strMid(sDataTemp,sTemp,2,12);
			     	pdIonAlpha[0] = atof(sDataTemp);
			    	strMid(sDataTemp,sTemp,2+12,12);
				    pdIonAlpha[1] = atof(sDataTemp);
			    	strMid(sDataTemp,sTemp,2+2*12,12);
			    	pdIonAlpha[2] = atof(sDataTemp);
			    	strMid(sDataTemp,sTemp,2+3*12,12);
			    	pdIonAlpha[3] = atof(sDataTemp);
					}
					in.getline(sTemp,200);
					sCheck.assign(&sTemp[60],&sTemp[60+8]);
					if(sCheck == "ION BETA"){
					strMid(sDataTemp,sTemp,2,12);
			    	pdIonBeta[0] = atof(sDataTemp);
			    	strMid(sDataTemp,sTemp,2+12,12);
			    	pdIonBeta[1] = atof(sDataTemp);
			    	strMid(sDataTemp,sTemp,2+2*12,12);
			    	pdIonBeta[2] = atof(sDataTemp);
			    	strMid(sDataTemp,sTemp,2+3*12,12);
			    	pdIonBeta[3] = atof(sDataTemp);
					}
				}
				else if(sCheck != "ION ALPHA")
				{
				//	in.getline(sTemp,200);
					sCheck.assign(&sTemp[60],&sTemp[60+9]);
					if(sCheck == "DELTA-UTC"){
						strMid(sDataTemp,sTemp,3,19);
						DeltaUTC.A0 = atof(sDataTemp);
						strMid(sDataTemp,sTemp,3+19,19);
						DeltaUTC.A1 = atof(sDataTemp);
						strMid(sDataTemp,sTemp,3+2*19,9);
						DeltaUTC.T = atoi(sDataTemp);
						strMid(sDataTemp,sTemp,3+2*19+9,9);
						DeltaUTC.W = atoi(sDataTemp);
										
					}
					else{
						sCheck.assign(&sTemp[60],&sTemp[60+12]);
						if(sCheck == "LEAP SECOND") 
						{
							strMid(sDataTemp,sTemp,0,6);
							LeapSeconds = atoi(sDataTemp);
						}
						else{
							sCheck.assign(&sTemp[67],&sTemp[67+6]); 
								//用于判断是否为文件头结尾
		//					if(sCheck == "HEADER")
		//						cout<<"I read the header just now!\n";
						}
					}
							
				}
							
			}			 
		
 	return *this;    
	
} 


oneGPSNAVMSGREC& oneGPSNAVMSGREC::ReadRecData(ifstream& in)
{
	char stemp[255];
	char sDataTemp[20];
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,0,2);
	byPRN = atoi(sDataTemp);
	if(byPRN == 0)return *this;  //当卫星号为0时，表示已经没有可用的数据了

	//TIME
	strMid(sDataTemp,stemp,3,2);
	TOC.wYear = atoi(sDataTemp);
 	if(TOC.wYear < 81) TOC.wYear += 2000;


	strMid(sDataTemp,stemp,6,2);
	TOC.byMonth = atoi(sDataTemp);

	strMid(sDataTemp,stemp,9,2);
	TOC.byDay = atoi(sDataTemp);

	strMid(sDataTemp,stemp,12,2);
	TOC.byHour = atoi(sDataTemp);

	strMid(sDataTemp,stemp,15,2);
	TOC.byMinute= atoi(sDataTemp);

	strMid(sDataTemp,stemp,17,5);
	TOC.bySecond = atof(sDataTemp);

	
    //卫星钟
    strMid(sDataTemp,stemp,22,19);
	dClkBias = atof(sDataTemp);

	strMid(sDataTemp,stemp,41,19);
	dClkDrift = atof(sDataTemp);

	strMid(sDataTemp,stemp,60,19);
	dClkDriftRate = atof(sDataTemp);

	
//	cout<<endl ;
	//广播轨道1
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,3,19);
	dIODE = atof(sDataTemp);
	strMid(sDataTemp,stemp,22,19);
	dCrs = atof(sDataTemp);

	strMid(sDataTemp,stemp,41,19);
	dDeltaN = atof(sDataTemp);
	strMid(sDataTemp,stemp,60,19);
	dM0 = atof(sDataTemp);

	//广播轨道2
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,3,19);
	dCuc = atof(sDataTemp);
	strMid(sDataTemp,stemp,22,19);
	d_e = atof(sDataTemp);
	strMid(sDataTemp,stemp,41,19);
	dCus = atof(sDataTemp);
	strMid(sDataTemp,stemp,60,19);
	dSqrtA= atof(sDataTemp);
	//广播轨道3
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,3,19);
	dTOE = atof(sDataTemp);
	strMid(sDataTemp,stemp,22,19);
	dCic = atof(sDataTemp);
	strMid(sDataTemp,stemp,41,19);
	this->dOmega = atof(sDataTemp);
	strMid(sDataTemp,stemp,60,19);
	dCis= atof(sDataTemp);

 

	//广播轨道4
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,3,19);
	d_i0 = atof(sDataTemp);
	strMid(sDataTemp,stemp,22,19);
	dCrc = atof(sDataTemp);
	strMid(sDataTemp,stemp,41,19);
	this->d_w = atof(sDataTemp);
	strMid(sDataTemp,stemp,60,19);
	dOmegaDot= atof(sDataTemp);
 

	//广播轨道5
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,3,19);
	d_iDot = atof(sDataTemp);
	strMid(sDataTemp,stemp,22,19);
	dCodesOnL2Channel = atof(sDataTemp);
	strMid(sDataTemp,stemp,41,19);
	dGPSWeek = atof(sDataTemp);
	strMid(sDataTemp,stemp,60,19);
	dL2PDataFlag = atof(sDataTemp);

 

	//广播轨道6
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,3,19);
	dSVAccuraccy = atof(sDataTemp);
	strMid(sDataTemp,stemp,22,19);
	dSVHealth = atof(sDataTemp);
	strMid(sDataTemp,stemp,41,19);
	dTGD = atof(sDataTemp);
	strMid(sDataTemp,stemp,60,19);
	dIODC = atof(sDataTemp);

	//广播轨道7
	in.getline(stemp,200);
	strMid(sDataTemp,stemp,3,19);
	dTransmissionTimeOfMessage = atof(sDataTemp);
	strMid(sDataTemp,stemp,22,19);
	dFitInterval = atof(sDataTemp);
	strMid(sDataTemp,stemp,41,19);
	dSpare1 = atof(sDataTemp);
	strMid(sDataTemp,stemp,60,19);
	dSpare2 = atof(sDataTemp);


	return *this;
}




GPSNAVMSG::GPSNAVMSG(ifstream& navFileName)
{
	oneGPSNAVMSGREC* poneNavRec;// = new oneGPSNAVMSGREC;
	Header.ReadNavMsgHeader(navFileName); 


//new//////////////////////////0519
    GPSNAVMSGREC* pOneSVRec;


	vector<GPSNAVMSGREC*>::iterator iter_navList;  //建立遍历器

	while(!navFileName.eof())
	{
	    poneNavRec = new oneGPSNAVMSGREC;

 		poneNavRec->ReadRecData(navFileName);


		for(iter_navList = this->pNavList.begin() ; iter_navList != this->pNavList.end(); iter_navList ++)
		{
			if((*iter_navList)->prn == poneNavRec->byPRN)
			{
				(*iter_navList)->pOneSatNavRec.push_back(poneNavRec);
				break;
			}
		}
		if(iter_navList == this->pNavList.end())
		{
			pOneSVRec = new GPSNAVMSGREC;
			pOneSVRec->prn = poneNavRec->byPRN;
			pOneSVRec->pOneSatNavRec.push_back(poneNavRec);	
			
			this->pNavList.push_back(pOneSVRec);

		}

	}
	poneNavRec = new oneGPSNAVMSGREC;  //必不可少
	delete poneNavRec;

	pOneSVRec = new GPSNAVMSGREC;
	delete pOneSVRec;
};

int GPSNAVMSG::Size()  //共有几颗卫星的数据
{
	return this->pNavList.size();
}


GPSNAVMSG& GPSNAVMSG::GetNearTimeNavRec(int byPRN,GPSTIME &gTimeEpoch, 
										oneGPSNAVMSGREC& nearTimeNavRec)// new
{//获取最接近观测时刻的星历信息
	 
	vector<GPSNAVMSGREC*>::iterator iter_navList;  //建立遍历器
	int i = 0;
 
	if(byPRN == 0) 
	{
//		cout<<"The sat num is wrong!\n";
		return *this;
	}
	for(iter_navList = this->pNavList.begin();iter_navList != this->pNavList.end();iter_navList++)
	{
		if((*iter_navList)->prn != byPRN)
		{
			continue;
		}

//			cout<<"There is no record of this satellite!\n";

    	if((*iter_navList)->prn == byPRN)//选择卫星的特定的数据
		{//if 1		

	      vector<oneGPSNAVMSGREC*>::iterator iter_oneRec;
 		  GPSNAVMSGREC* pRecList = *iter_navList;
		
		  i = 0;
    	  int k = 0;

	      GPSTIME tTemp;
	      GPSTIME tTemp2;
		  tTemp.iWeek = pRecList->pOneSatNavRec[i]->dGPSWeek; //第一个数据的时间信息
 	      tTemp.ldSecond = pRecList->pOneSatNavRec[i]->dTOE;
		  int numOfRec = pRecList->pOneSatNavRec.size();

		  for(iter_oneRec = pRecList->pOneSatNavRec.begin(); iter_oneRec != pRecList->pOneSatNavRec.end();
			iter_oneRec++)
			{//for1
			   i++;
			   if(i == numOfRec) break;
    		  tTemp2.iWeek = pRecList->pOneSatNavRec[i]->dGPSWeek;
			  tTemp2.ldSecond = pRecList->pOneSatNavRec[i]->dTOE;
			  if(fabs(tTemp2-gTimeEpoch) < fabs(tTemp-gTimeEpoch))
			  {
				k = i;
				tTemp = tTemp2;

			  }
			
			}//end for1
		nearTimeNavRec =  *pRecList->pOneSatNavRec[k]  ;
		}
	}

		


//		}//end if1
	return *this;
}

double oneGPSNAVMSGREC::GetSVClkBias(const GPSTIME &gTimeEpoch) //new
{
	double SVClkBias;

	GPSTIME gpsToc;
	this->TOC.ConvertToGpsTime(gpsToc);
	
	double temp;
 	temp = gTimeEpoch.ldSecond - gpsToc.ldSecond;

	if(temp > 302400) temp -= 604800;
	if(temp < -302400)  temp += 604800;

	SVClkBias = this->dClkBias  + this->dClkDrift  * temp +
 	   this->dClkDriftRate  * temp  * temp;	

 

	return SVClkBias;

}

void oneGPSNAVMSGREC::GetSVPos(GPSTIME& shootTime,double &X, double &Y, double &Z)//new
{
	  
	double n0,n;              /* mean rate of angle */
	double tk, mk, ek1, ek2, ek;
	double Error=1.0e-12;
	double vk, phik;
	double deltau,deltar,deltai;
	double uk, rk, ik;
	double xk, yk;
	double omegak;

    double omegae=7.2921151467e-5;  /* earth self round rate of angle */   //by liu2004/10/18

	/* compute SV mean rate of angle */
	n0 = sqrt(GM) / (this->dSqrtA * this->dSqrtA * this->dSqrtA);
	n  = n0 + this->dDeltaN ;

	/* compute time tk */
	tk = shootTime.ldSecond - this->dTOE;
	if(tk > 302400) tk -= 604800;
	else if(tk < -302400) tk +=604800;

	/* compute mean anomoly at measure time */
	mk = this->dM0  + n * tk;

	/* computer ek */
	ek1 = mk;
	do
	{
		ek2 = mk + this->d_e * sin(ek1);//Text->e * sin(ek1);
		if(fabs(ek2-ek1)<=Error ) break;
		ek1=ek2;
	}while(1);
	ek=ek1;

	this->Ek = ek; //将ek存放在对象中

	/* computer vk */
	vk = atan2( sqrt ((1.0-this->d_e * this->d_e)) * sin(ek)   , cos(ek) - this->d_e );

	/* compute phik */
	phik = vk + this->d_w;//Text->omega ;

	/* compute deltau,deltar,deltai */
	deltau = this->dCuc * cos(2.0*phik) + this->dCus *sin(2.0*phik) ;
	deltar = this->dCrc * cos(2.0*phik) + this->dCrs *sin(2.0*phik) ;
	deltai = this->dCic * cos(2.0*phik) + this->dCis *sin(2.0*phik) ;

	/* computer uk, rk and ik */
	uk = phik +deltau;
	rk = this->dSqrtA * this->dSqrtA *(1.0 - this->d_e * cos(ek)) + deltar;
	ik = this->d_i0 + deltai + this->d_iDot * tk;

	// compute xk,yk 
	xk = rk * cos(uk);
	yk = rk * sin(uk);

	// compute omegak 
	omegak = this->dOmega + (this->dOmegaDot - w_e)*tk - w_e * this->dTOE;
//	omegak = Text->omega0  + (Text->omegadot  - omegae)*tk -omegae * Text->toe ;

	// compute ECEF
	X = xk * cos(omegak) - yk * cos(ik) *sin(omegak) ;
	Y = xk * sin(omegak) + yk * cos(ik) *cos(omegak) ;
	Z = yk * sin(ik) ;

}

//张老师课件
double oneGPSNAVMSGREC::GetSVClkRelation()
{
	return -2289.7 * 1.0e-10 * this->d_e * sin(this->Ek);
}

GPSNAVMSGREC::~GPSNAVMSGREC()
{
 	for_each(this->pOneSatNavRec.begin(),this->pOneSatNavRec.end(),CDeleteObject());
	this->pOneSatNavRec.clear();

}

GPSNAVMSG::~GPSNAVMSG()
{
	for_each(this->pNavList.begin(),this->pNavList.end(),CDeleteObject());
	this->pNavList.clear();
}

GPSNAVMSGREC::GPSNAVMSGREC()
{

}

GPSNAVMSGREC::GPSNAVMSGREC(const GPSNAVMSGREC &r)
{
	this->prn = r.prn;
	this->pOneSatNavRec.assign(r.pOneSatNavRec.begin(),r.pOneSatNavRec.end());
}

double oneGPSNAVMSGREC::GetIonDelay(double& E)
{//参见硕士论文《GPS单点定位研究》page 47 
 //用于伪距电离层改正


	/*  导航电文没有提供电离层延迟参数
	//B L为测站的地心经纬度
	
	double fi_k;
	double n_k;
      
	double fi_m;//地磁纬度（度）

	double UT;  //单位为秒
	double t_dot;

	double A; //电离层延迟函数振幅
	double p; //电离层延迟函数周期

	double E_rad = E * 180/PI;
	double a_rad = a * 180/PI;

	Ea_deg = 445/(Ea_deg + 20) - 4;
	fi_k = B + Ea_deg * cos(a);/////////////////////maybe wrong
	n_k = L + Ea_deg * sin(a) /cos(fi_k);


	UT = gTime.ldSecond;

	while(gTime.ldSecond > 86400)
	{
		UT -= 86400; 
	}

	t_dot = UT + n_k * 3600 /15;

    fi_m = fi_k + 11.6 * cos((n_k - 291)*PI/180);
	*/


	double IonDelay;
	double SF;
	SF = 1 + 2 * pow( (96 - E * 180/PI) / 90 , 3.0 );

	IonDelay = SF * this->dTGD * VC;

	return IonDelay;
	

}