📄 rinexnav.cpp
字号:
#include "RinexNav.h"
#include "stdAfx.h"
#include "GlobleDefine.h"
#include "fstream.h"
#include "TimeCorTran.h"
#include "math.h"
#include "ErrorCorrection.h"
void READNAV(ifstream inNavFile,ofstream outPosFile,NAVHEAD header,NAV *nav,int &TOTALNAVNUM)
{
char str[81];
CString s;
//char *p;
do
{
inNavFile.getline(str,81,'\n');
if(strstr(str,"ION ALPHA")){
s=str;
header.dIONalph0=atof(s.Mid(5,12));
header.dIONalph1=atof(s.Mid(17,12));
header.dIONalph2=atof(s.Mid(29,12));
header.dIONalph3=atof(s.Mid(41,12));
}
if(strstr(str,"ION BETA")){
s=str;
header.dIONbelta0=atof(s.Mid(5,12));
header.dIONbelta1=atof(s.Mid(17,12));
header.dIONbelta2=atof(s.Mid(29,12));
header.dIONbelta3=atof(s.Mid(41,12));}
if(strstr(str,"DELTA-UTC")){
s=str;
header.dA0=atof(s.Mid(5,19));
header.dA1=atof(s.Mid(24,19));
header.lseconds=(long)atoi(s.Mid(43));
header.lweek=(long)atof(s.Mid(52));}
if(strstr(str,"LEAP SECONDS")){
s=str;
header.iLEAPSECONDS =atoi(s.Mid(4));}
}while(!strstr(str,"END OF HEADER"));
//du ji lu wen jian
//if((nav=new NAV[200])==NULL)
// cerr<<"Error allocating memory!"<<endl;
int i=0,len;
while(1)
{ //if((nav=new NAV[200])==NULL)
//cerr<<"Error allocating memory!"<<endl;
depart:;
if(inNavFile.peek()==EOF)
break;
if(inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
{ s=str;
nav[i].iPRN=atoi(s.Mid(0,2));
nav[i].UTC.iYEAR=atoi(s.Mid(2,3));
nav[i].UTC.iMONTH=atoi(s.Mid(5,3));
nav[i].UTC.iDAY=atoi(s.Mid(8,3));
nav[i].UTC.iHOUR=atoi(s.Mid(11,3));
nav[i].UTC.iMINUTE=atoi(s.Mid(14,3));
nav[i].UTC.dSECOND=atof(s.Mid(17,5));;
nav[i].dDELTASV=atof(s.Mid(22,19));
nav[i].dDELTASV1=atof(s.Mid(41,19));
nav[i].dDELTASV2=atof(s.Mid(60,19));
}
if(len==0)
goto depart;
// BROADCAST ORBIT -1
if(inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
{s=str;
nav[i].dIODE =(long)atof(s.Mid(3,19));
nav[i].dcrs =atof(s.Mid(22,19));
nav[i].dDeltaN=atof(s.Mid(41,19));
nav[i].dM0 =atof(s.Mid(60,19));
}
// BROADCAST ORBIT -2
if( inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
{s=str;
nav[i].dcuc =atof(s.Mid(3,19));
nav[i].dE =atof(s.Mid(22,19));
nav[i].dcus =atof(s.Mid(41,19));
nav[i].dRoota =atof(s.Mid(60,19));
}
// BROADCAST ORBIT -3
if( inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
{s=str;
nav[i].dtoe =(long)atof(s.Mid(3,19));//*zxf
nav[i].dcic =atof(s.Mid(22,19));
nav[i].dOMEGA =atof(s.Mid(41,19));
nav[i].dcis =atof(s.Mid(60,19));
}
// BROADCAST ORBIT -4
if( inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
{s=str;
nav[i].DI0 =atof(s.Mid(3,19));
nav[i].dcrc =atof(s.Mid(22,19));
nav[i].dOmega =atof(s.Mid(41,19));
nav[i].dOMEGADOT =atof(s.Mid(60,19));
}
// BROADCAST ORBIT -5
if( inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
{s=str;
nav[i].dIDOT =atof(s.Mid(3,19));
nav[i].lWEEK =(short)atof(s.Mid(41,19));//*zxf
}
// BROADCAST ORBIT -6
if( inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
{s=str;
nav[i].iACCURACY =atoi(s.Mid(3,19));
nav[i].iHEALTH =atoi(s.Mid(22,19));
nav[i].dTGD =(double)atof(s.Mid(41,19)); }
// BROADCAST ORBIT -7
if(inNavFile.getline(str,81,'\n')&&(len=strlen(str)))
s=str;
else
continue;
i++;
if(i==163)
continue;
TOTALNAVNUM=i;
}
//delete[]nav;
}
//计算t 时刻卫星的位置和速度
void GETSVPOSFROMNAV(double t,NAV *nav,SATPOS &satpos)
{ //double omega = 7.2921151467E-5 ;//地球自转角速度
//double mu=3.986005e14; //地球引力常数 Earth constant , unit m^3/s^2
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
satpos.time.dWEEKSECONDS=t;
// compute SV mean rate of angle
n0 = sqrt(mu) / (nav->dRoota* nav->dRoota * nav->dRoota);//计算平均角速度
n = n0 + nav->dDeltaN ;//改正平均角速度
//compute time tk 计算从需要时刻到参考时刻的时间差
tk = satpos.time.dWEEKSECONDS - nav->dtoe;
if(tk > 302400) tk -= 604800;
else if(tk < -302400) tk +=604800;
//shi jisuan shi ke yu Toe bu chao guo 3.5 tian
// compute mean anomoly at measure time 计算平近点角
mk = nav->dM0 + n * tk;
//computer ek 迭代计算偏近点角
ek1 = mk;
do
{
ek2 = mk + nav->dE * sin(ek1);
if(fabs(ek2-ek1)<=Error ) break;
ek1=ek2;
}while(1);
ek=ek1;
// computer vk 计算真近点角
vk = Get_atan(cos(ek)-nav->dE,sqrt(1.0-nav->dE *nav->dE)*sin(ek));
// compute phik 计算纬度参数
phik = vk + nav->dOmega ;
// compute deltau,deltar,deltai 周期改正项
deltau = nav->dcuc * cos(2.0*phik) + nav->dcus *sin(2.0*phik) ;
deltar = nav->dcrc * cos(2.0*phik) + nav->dcrs *sin(2.0*phik) ;
deltai = nav->dcic * cos(2.0*phik) + nav->dcis *sin(2.0*phik) ;
// computer uk, rk and ik 计算改正后的纬度参数,向经和倾角
uk = phik +deltau;
rk = nav->dRoota *nav->dRoota * (1.0-nav->dE * cos(ek)) + deltar ;
ik = nav->DI0 + deltai + nav->dIDOT * tk ;
//compute xk,yk 计算卫星在轨道平面内的坐标
xk = rk * cos(uk);
yk = rk * sin(uk);
///compute omegak 改正升交点的经度
omegak = nav->dOMEGA + (nav->dOMEGADOT - omegae)*tk -omegae * nav->dtoe ;
//compute X,Y,Z 计算卫星在地固系WGS84中的坐标
satpos.xyz.lX = xk * cos(omegak) - yk * cos(ik) *sin(omegak) ;
satpos.xyz.lY = xk * sin(omegak) + yk * cos(ik) *cos(omegak) ;
satpos.xyz.lZ = yk * sin(ik) ;
satpos.iPRN=nav->iPRN;
//compute the dX,dY,dZ of the satelite
double Es1,u01,u1,r1,I1,x0,y0,u,r,i,x01,y01;
double u0,fs,Es,es,as,L,L1;
fs = vk ;
u0 = phik;
Es = ek ;
es = nav->dE;
as = nav->dRoota * nav->dRoota;
u = uk;
r = rk;
i = ik;
x0 = xk;
y0 = yk;
L = omegak;
Es1 = ( n )/(1.0 - es*cos(Es)); // FORMULA WRONG HERE
u01 = sqrt((1+es)/(1-es))*((cos(fs/2.0)*cos(fs/2.0))/(cos(Es/2)*cos(Es/2)))*Es1;
u1 = (1+2*nav->dcus*cos(2*u0) - 2*nav->dcuc*sin(2*u0))*u01;
r1 = as*es*sin(Es)*Es1+2*(nav->dcrs*cos(2*u0)-nav->dcrc*sin(2*u0))*u01;
I1 = nav->dIDOT + 2*(nav->dcis*cos(2*u0)-nav->dcic*sin(2*u0))*u01;
L1 = nav->dOMEGADOT - omega;
x01 = cos(u)*r1-r*sin(u)*u1;
y01 = sin(u)*r1+r*cos(u)*u1;
// *drel=-2*sqrt(mu*nav->roota)/SPEED_OF_LIGHT*nav->e*sin(ek);
// *drel=-2*sqrt(mu*as)/SPEED_OF_LIGHT*nav->e*sin(ek);
//nav->satvel[0]=cos(L)*x01-sin(L)*cos(i)*y01-(x0*sin(L)+y0*cos(L)*cos(i))*L1+y0*sin(L)*sin(i)*I1;
//nav->satvel[1]=sin(L)*x01+cos(L)*cos(i)*y01+(x0*cos(L)-y0*sin(L)*cos(i))*L1-y0*cos(L)*sin(i)*I1;
//nav->satvel[2]= 0 +sin(i)* y01+ 0 +y0*cos(i)* I1;
satpos.Dotxyz.lX=cos(L)*x01-sin(L)*cos(i)*y01-(x0*sin(L)+y0*cos(L)*cos(i))*L1+y0*sin(L)*sin(i)*I1;
satpos.Dotxyz.lY=sin(L)*x01+cos(L)*cos(i)*y01+(x0*cos(L)-y0*sin(L)*cos(i))*L1-y0*cos(L)*sin(i)*I1;
satpos.Dotxyz.lZ= 0 +sin(i)* y01+ 0 +y0*cos(i)* I1;
satpos.time.lGPSWEEK = nav->lWEEK;
// add GPSWEEK in 2007.4.26
}
void GETFINALSVPOSFRONAV(long double &t,int &iPRN ,int inear,NAV *nav,SATPOS &satpos,long double &obsx,long double &obsy,long double &obsz)
{ double ts,ts0;
double distance;
double x1=0;
double y1=0;
double z1=0;
ts=t-0.075;
do{
ts0=ts;
GETSVPOSFROMNAV(ts0,nav+inear,satpos);
distance=SSDistance(obsx,obsy,obsz,satpos.xyz.lX,satpos.xyz.lY,satpos.xyz.lZ);
if((!obsx)||(!obsy)||(!obsz))
break;
ts=t-distance/VC;
}while(fabs(ts-ts0)>0.0000001);
//注：不能太小，否则迭代不出来
//return(inear);
Earthrotation(t-ts0,&satpos);
}
int FindNearToe(double dEPOCH,int iPRN,long lTotalsatnum,NAV *nav)
{
double mintimespan=1000000000;
int iFindneartoe=0;
for(int i=0;i<lTotalsatnum;i++)
{
if(iPRN==nav[i].iPRN)
{
double tempmin=fabs(nav[i].dtoe-dEPOCH);
if(tempmin<mintimespan)
{
iFindneartoe=i;
mintimespan=tempmin;
}
}
}
return(iFindneartoe);
}⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -