mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-06-18 11:09:56 +00:00
Code Issues Releases Wiki Activity

Make code more readable

Browse Source
This commit is contained in:
Carles Fernandez 2017-05-06 13:17:27 +02:00
parent 3f88fae88f
commit 9ee11b691d
2 changed files with 56 additions and 44 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
src/algorithms/libs/rtklib/rtklib_rtkcmn.cc
View File

@ -454,15 +454,27 @@ int satexclude(int sat, int svh, const prcopt_t *opt)
{ {
int sys = satsys(sat, NULL); int sys = satsys(sat, NULL);
if (svh<0) return 1; /* ephemeris unavailable */ if (svh < 0)
{
trace(3, "ephemeris unavailable: sat=%3d svh=%02X\n", sat, svh);
return 1; /* ephemeris unavailable */
}
if (opt) if (opt)
{ {
if (opt->exsats[sat-1] == 1) return 1; /* excluded satellite */ if (opt->exsats[sat-1] == 1)
{
trace(3, "excluded satellite: sat=%3d svh=%02X\n", sat, svh);
return 1; /* excluded satellite */
}
if (opt->exsats[sat-1] == 2) return 0; /* included satellite */ if (opt->exsats[sat-1] == 2) return 0; /* included satellite */
if (!(sys&opt->navsys)) return 1; /* unselected sat sys */ if (!(sys&opt->navsys))
{
trace(3, "unselected sat sys: sat=%3d svh=%02X\n", sat, svh);
return 1; /* unselected sat sys */
}
} }
if (sys == SYS_QZS) svh&=0xFE; /* mask QZSS LEX health */ if (sys == SYS_QZS) svh &= 0xFE; /* mask QZSS LEX health */
if (svh) if (svh)
{ {
trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh); trace(3, "unhealthy satellite: sat=%3d svh=%02X\n", sat, svh);
@ -3132,12 +3144,12 @@ void traceobs(int level __attribute__((unused)), const obsd_t *obs __attribute__
//void tracelevel(int level) {} //void tracelevel(int level) {}
void trace (int level __attribute__((unused)), const char *format __attribute__((unused)), ...) void trace (int level __attribute__((unused)), const char *format __attribute__((unused)), ...)
{ {
// va_list ap; /*va_list ap;*/
/* print error message to stderr */ /* print error message to stderr */
// printf("RTKLIB TRACE[%i]:",level); /*printf("RTKLIB TRACE[%i]:",level);
// va_start(ap,format); va_start(ap,format);
// vfprintf(stdout,format,ap); vfprintf(stdout,format,ap);
// va_end(ap); va_end(ap);*/
} }
//void tracet (int level, const char *format, ...) {} //void tracet (int level, const char *format, ...) {}
//void tracemat(int level, const double *A, int n, int m, int p, int q) {} //void tracemat(int level, const double *A, int n, int m, int p, int q) {}
@ -4002,50 +4014,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, /* From RTKLIB 2.4.2 */
double *phw) 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 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}; double dr[3], ds[3], drs[3], r[3], pos[3], rsun[3], cosp, ph, erpv[5]={0};
int i; int i;
trace(4,"windupcorr: time=%s\n",time_str(time,0)); trace(4, "windupcorr: time=%s\n", time_str(time, 0));
/* sun position in ecef */ /* sun position in ecef */
sunmoonpos(gpst2utc(time),erpv,rsun,NULL,NULL); sunmoonpos(gpst2utc(time), erpv, rsun, NULL, NULL);
/* unit vector satellite to receiver */ /* unit vector satellite to receiver */
for (i=0;i<3;i++) r[i]=rr[i]-rs[i]; for (i = 0; i < 3; i++) r[i] = rr[i]-rs[i];
if (!normv3(r,ek)) return; if (!normv3(r, ek)) return;
/* unit vectors of satellite antenna */ /* unit vectors of satellite antenna */
for (i=0;i<3;i++) r[i]=-rs[i]; for (i = 0; i < 3; i++) r[i] = -rs[i];
if (!normv3(r,ezs)) return; if (!normv3(r, ezs)) return;
for (i=0;i<3;i++) r[i]=rsun[i]-rs[i]; for (i = 0; i < 3; i++) r[i] = rsun[i]-rs[i];
if (!normv3(r,ess)) return; if (!normv3(r, ess)) return;
cross3(ezs,ess,r); cross3(ezs, ess, r);
if (!normv3(r,eys)) return; if (!normv3(r, eys)) return;
cross3(eys,ezs,exs); cross3(eys, ezs, exs);
/* unit vectors of receiver antenna */ /* unit vectors of receiver antenna */
ecef2pos(rr,pos); ecef2pos(rr, pos);
xyz2enu(pos,E); xyz2enu(pos, E);
exr[0]= E[1]; exr[1]= E[4]; exr[2]= E[7]; /* x = north */ 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 */ eyr[0] = -E[0]; eyr[1] = -E[3]; eyr[2] = -E[6]; /* y = west */
/* phase windup effect */ /* phase windup effect */
cross3(ek,eys,eks); cross3(ek, eys, eks);
cross3(ek,eyr,ekr); cross3(ek, eyr, ekr);
for (i=0;i<3;i++) { 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]; 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; cosp = dot(ds, dr, 3) / norm_rtk(ds, 3) / norm_rtk(dr, 3);
else if (cosp> 1.0) cosp= 1.0; if (cosp < -1.0) cosp = -1.0;
ph=acos(cosp)/2.0/PI; else if (cosp > 1.0) cosp = 1.0;
cross3(ds,dr,drs); ph = acos(cosp)/2.0/PI;
if (dot(ek,drs,3)<0.0) ph=-ph; cross3(ds, dr, drs);
if (dot(ek, drs, 3) < 0.0) ph = -ph;
*phw=ph+floor(*phw-ph+0.5); /* in cycle */ *phw = ph + floor(*phw - ph + 0.5); /* in cycle */
} }

3
src/algorithms/libs/rtklib/rtklib_rtkcmn.h
View File

@ -266,7 +266,6 @@ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun,
void csmooth(obs_t *obs, int ns); void csmooth(obs_t *obs, int ns);
int rtk_uncompress(const char *file, char *uncfile); int rtk_uncompress(const char *file, char *uncfile);
int expath(const char *path, char *paths[], int nmax); int expath(const char *path, char *paths[], int nmax);
void windupcorr(gtime_t time, const double *rs, const double *rr, void windupcorr(gtime_t time, const double *rs, const double *rr, double *phw);
double *phw);
#endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */ #endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */