Some required changes to use 2.4.2 ppp funcitions

This commit is contained in:
Carles Fernandez 2017-05-05 13:51:42 +02:00
parent 1e390a7baa
commit 182fb02e7f
1 changed files with 48 additions and 0 deletions

View File

@ -4000,3 +4000,51 @@ int expath(const char *path, char *paths[], int nmax)
return n;
}
void windupcorr(gtime_t time, const double *rs, const double *rr,
double *phw)
{
double ek[3],exs[3],eys[3],ezs[3],ess[3],exr[3],eyr[3],eks[3],ekr[3],E[9];
double dr[3],ds[3],drs[3],r[3],pos[3],rsun[3],cosp,ph,erpv[5]={0};
int i;
trace(4,"windupcorr: time=%s\n",time_str(time,0));
/* sun position in ecef */
sunmoonpos(gpst2utc(time),erpv,rsun,NULL,NULL);
/* unit vector satellite to receiver */
for (i=0;i<3;i++) r[i]=rr[i]-rs[i];
if (!normv3(r,ek)) return;
/* unit vectors of satellite antenna */
for (i=0;i<3;i++) r[i]=-rs[i];
if (!normv3(r,ezs)) return;
for (i=0;i<3;i++) r[i]=rsun[i]-rs[i];
if (!normv3(r,ess)) return;
cross3(ezs,ess,r);
if (!normv3(r,eys)) return;
cross3(eys,ezs,exs);
/* unit vectors of receiver antenna */
ecef2pos(rr,pos);
xyz2enu(pos,E);
exr[0]= E[1]; exr[1]= E[4]; exr[2]= E[7]; /* x = north */
eyr[0]=-E[0]; eyr[1]=-E[3]; eyr[2]=-E[6]; /* y = west */
/* phase windup effect */
cross3(ek,eys,eks);
cross3(ek,eyr,ekr);
for (i=0;i<3;i++) {
ds[i]=exs[i]-ek[i]*dot(ek,exs,3)-eks[i];
dr[i]=exr[i]-ek[i]*dot(ek,exr,3)+ekr[i];
}
cosp=dot(ds,dr,3)/norm_rtk(ds,3)/norm_rtk(dr,3);
if (cosp<-1.0) cosp=-1.0;
else if (cosp> 1.0) cosp= 1.0;
ph=acos(cosp)/2.0/PI;
cross3(ds,dr,drs);
if (dot(ek,drs,3)<0.0) ph=-ph;
*phw=ph+floor(*phw-ph+0.5); /* in cycle */
}