timecortran.cpp

GPS 定位授时源码

#include "TimeCorTran.h"

#include "StdAfx.h"
#include "math.h"
#include "GlobleDefine.h"

void UTCCOVGPS(UTCTIME *ptrTime,GPSTIME *ptr)
{
   //
	int iDAYOFMONTH[13]={0,31,28,31,30,31,30,31,31,30,31,30,31};

	int year,month,day,hour,minute; 
	double second,weeksec;

	year=ptrTime->iYEAR+2000;
	month=ptrTime->iMONTH;
	day=ptrTime->iDAY;
	hour=ptrTime->iHOUR;
	minute=ptrTime->iMINUTE;
	second=ptrTime->dSECOND;


	int   dayofw,dayofy, yr, ttlday, m, weekno;

	// Check limits of day, month and year 
	if (year < 1981 || month < 1 || month > 12 || day < 1 || day > 31)
		weekno = 0;

   //  Convert day, month and year to day of year 
	if (month == 1)
		dayofy = day;
	else
	{
		dayofy = 0;
		for (m=1; m<=(month-1); m++)
		{
			dayofy += iDAYOFMONTH[m];
			if ( m==2 )
			{
				if (year % 4 == 0 && year % 100 != 0 || year % 400 == 0)
					dayofy += 1;
			}
		}
		dayofy += day;
	}

	// Convert day of year and year into week number and day of week 
	ttlday = 360;//GPS系统开始时间1980年1月6日(星期日)0时
	for (yr=1981; yr<=(year-1); yr++)
	{
		ttlday  += 365;
		if (yr % 4 == 0 && yr % 100 != 0 || yr % 400 ==0)
			ttlday  += 1;
	}
	ttlday  += dayofy;
	weekno   = ttlday/7;
	dayofw   = ttlday - 7 * weekno;

	weeksec =  (hour * 3600 + minute * 60 + second + dayofw * 86400);//从星期日起算
	ptr->lGPSWEEK=weekno;
	ptr->dWEEKSECONDS=weeksec;
} 

		 
void  RECXYZ_BLH(RECPOS &recpos)
{	
	//XYZ->BLH
	double B0,R,N;
	R=sqrt(recpos.xyz.lX*recpos.xyz.lX+recpos.xyz.lY*recpos.xyz.lY);	
	B0=atan2(recpos.xyz.lZ,R);
	while(1)
	{
		N=ellips_a/sqrt(1.0-ellips_e2*sin(B0)*sin(B0));

		recpos.blh.lB=atan2(recpos.xyz.lZ+N*ellips_e2*sin(B0),R); //in radian

		if(fabs(recpos.blh.lB- B0 )<1.0e-16 ) break;
		
		B0 =recpos.blh.lB ;
	}
	recpos.blh.lL=atan2(recpos.xyz.lY,recpos.xyz.lX);//in radian
	recpos.blh.lH=R/cos(recpos.blh.lB)-N;
}



double Get_atan(double z,double y)
{
   double x;
   if (z==0) x=PI/2;
   else
   {
	   if (y==0) x=PI;
	   else
	   {
		   x=atan(fabs(y/z));
		   if ((y>0)&&(z<0)) x=PI-x;
		   else if ((y<0)&&(z<0)) x=PI+x;
		   else if((y<0)&&(z>0)) x=2*PI-x;
	   }
   }
   return x;
}

void   SATXYZ_AE(SATPOS &satpos,RECPOS &recpos)
{
	//BLH->NEU
	//also gets Elevation and azimuth


  	double dDeltaX = satpos.xyz.lX-recpos.xyz.lX;
	double dDeltaY = satpos.xyz.lY-recpos.xyz.lY;
	double dDeltaZ = satpos.xyz.lZ-recpos.xyz.lZ;

	double H[3][3];                 //旋转矩阵
  	
	H[0][0]=-sin(recpos.blh.lB)*cos(recpos.blh.lL);
	H[0][1]=-sin(recpos.blh.lB)*sin(recpos.blh.lL);
	H[0][2]= cos(recpos.blh.lB);

	H[1][0]=-sin(recpos.blh.lL);
	H[1][1]= cos(recpos.blh.lL);
	H[1][2]= 0.0;

	H[2][0]=cos(recpos.blh.lB)*cos(recpos.blh.lL);
	H[2][1]=cos(recpos.blh.lB)*sin(recpos.blh.lL);
	H[2][2]=sin(recpos.blh.lB);

	double dDeltaN=H[0][0]*dDeltaX+H[0][1]*dDeltaY+H[0][2]*dDeltaZ;
	double dDeltaE=H[1][0]*dDeltaX+H[1][1]*dDeltaY+H[1][2]*dDeltaZ;
	double dDeltaU=H[2][0]*dDeltaX+H[2][1]*dDeltaY+H[2][2]*dDeltaZ;


  	double s = dDeltaU / sqrt(dDeltaN * dDeltaN + dDeltaE* dDeltaE + dDeltaU * dDeltaU);

  	satpos.dE = Get_atan(sqrt(1.0 - s * s), s);
	satpos.dE = satpos.dE*180/PI;   //transform to degree
	satpos.dA = Get_atan(dDeltaE , dDeltaN);
	satpos.dA = satpos.dA*180/PI;   //transform to degree
}


//求距函数
long double SSDistance(double x1,double y1,double z1,double x2,double y2,double z2)
{
     double ssdistance=0;
	 ssdistance=sqrt((x2-x1)*(x2-x1)+(y2-y1)*(y2-y1)+(z2-z1)*(z2-z1));
	 return(ssdistance);
}