mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2025-11-20 00:55:17 +00:00
Some required changes to use 2.4.2 ppp funcitions
This commit is contained in:
@@ -4000,3 +4000,51 @@ int expath(const char *path, char *paths[], int nmax)
|
|||||||
|
|
||||||
return n;
|
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 */
|
||||||
|
}
|
||||||
|
|||||||
Reference in New Issue
Block a user