mirror of
https://github.com/gnss-sdr/gnss-sdr
synced 2024-12-14 20:20:35 +00:00
Make code more readable
This commit is contained in:
Carles Fernandez
parent
3f88fae88f
commit
9ee11b691d
@ -454,15 +454,27 @@ int satexclude(int sat, int svh, const prcopt_t *opt)
|
||||
{
|
||||
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->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 (!(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)
|
||||
{
|
||||
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 trace (int level __attribute__((unused)), const char *format __attribute__((unused)), ...)
|
||||
{
|
||||
// va_list ap;
|
||||
/* print error message to stderr */
|
||||
// printf("RTKLIB TRACE[%i]:",level);
|
||||
// va_start(ap,format);
|
||||
// vfprintf(stdout,format,ap);
|
||||
// va_end(ap);
|
||||
/*va_list ap;*/
|
||||
/* print error message to stderr */
|
||||
/*printf("RTKLIB TRACE[%i]:",level);
|
||||
va_start(ap,format);
|
||||
vfprintf(stdout,format,ap);
|
||||
va_end(ap);*/
|
||||
}
|
||||
//void tracet (int level, const char *format, ...) {}
|
||||
//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;
|
||||
}
|
||||
|
||||
void windupcorr(gtime_t time, const double *rs, const double *rr,
|
||||
double *phw)
|
||||
/* From RTKLIB 2.4.2 */
|
||||
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};
|
||||
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));
|
||||
trace(4, "windupcorr: time=%s\n", time_str(time, 0));
|
||||
|
||||
/* sun position in ecef */
|
||||
sunmoonpos(gpst2utc(time),erpv,rsun,NULL,NULL);
|
||||
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;
|
||||
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);
|
||||
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 */
|
||||
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;
|
||||
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 */
|
||||
*phw = ph + floor(*phw - ph + 0.5); /* in cycle */
|
||||
}
|
||||
|
||||
@ -266,7 +266,6 @@ void sunmoonpos(gtime_t tutc, const double *erpv, double *rsun,
|
||||
void csmooth(obs_t *obs, int ns);
|
||||
int rtk_uncompress(const char *file, char *uncfile);
|
||||
int expath(const char *path, char *paths[], int nmax);
|
||||
void windupcorr(gtime_t time, const double *rs, const double *rr,
|
||||
double *phw);
|
||||
void windupcorr(gtime_t time, const double *rs, const double *rr, double *phw);
|
||||
|
||||
#endif /* GNSS_SDR_RTKLIB_RTKCMN_H_ */
|
||||
|
||||
Loading…
Reference in New Issue
Block a user