mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2025-01-16 12:12:57 +00:00
Code Issues Releases Wiki Activity

Remove some warnings in rtkpos and make it more readable

Browse Source
This commit is contained in:
Carles Fernandez 2017-04-30 09:08:19 +02:00
parent 9323360526
commit cd87e070a4
2 changed files with 687 additions and 702 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

33
src/algorithms/libs/rtklib/rtklib_rtkpos.cc
View File

@ -380,10 +380,10 @@ double gfobs_L1L5(const obsd_t *obs, int i, int j, const double *lam)
/* single-differenced measurement error variance -----------------------------*/ /* single-differenced measurement error variance -----------------------------*/
double varerr(int sat, int sys, double el, double bl, double dt, int f, double varerr(int sat __attribute((unused)), int sys, double el, double bl, double dt, int f,
const prcopt_t *opt) const prcopt_t *opt)
{ {
double a, b, c = opt->err[3]*bl/1E4, d = SPEED_OF_LIGHT*opt->sclkstab*dt, fact = 1.0; double a, b, c = opt->err[3]*bl/1e4, d = SPEED_OF_LIGHT * opt->sclkstab*dt, fact = 1.0;
double sinel = sin(el); double sinel = sin(el);
int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0), nf = NF_RTK(opt); int i = sys == SYS_GLO ? 1 : (sys == SYS_GAL ? 2 : 0), nf = NF_RTK(opt);
@ -559,7 +559,7 @@ void udion(rtk_t *rtk, double tt, double bl, const int *sat, int ns)
/* temporal update of tropospheric parameters --------------------------------*/ /* temporal update of tropospheric parameters --------------------------------*/
void udtrop(rtk_t *rtk, double tt, double bl) void udtrop(rtk_t *rtk, double tt, double bl __attribute((unused)))
{ {
int i, j, k; int i, j, k;
@ -575,7 +575,7 @@ void udtrop(rtk_t *rtk, double tt, double bl)
if (rtk->opt.tropopt >= TROPOPT_ESTG) if (rtk->opt.tropopt >= TROPOPT_ESTG)
{ {
for (k = 0;k<2;k++) initx(rtk, 1E-6, VAR_GRA, ++j); for (k = 0;k<2;k++) initx(rtk, 1e-6, VAR_GRA, ++j);
} }
} }
else else
@ -1133,7 +1133,7 @@ double prectrop(gtime_t time, const double *pos, int r,
/* glonass inter-channel bias correction -------------------------------------*/ /* glonass inter-channel bias correction -------------------------------------*/
double gloicbcorr(int sat1, int sat2, const prcopt_t *opt, double lam1, double gloicbcorr(int sat1 __attribute((unused)), int sat2 __attribute((unused)), const prcopt_t *opt, double lam1,
double lam2, int f) double lam2, int f)
{ {
double dfreq; double dfreq;
@ -1436,12 +1436,10 @@ int ddmat(rtk_t *rtk, double *D)
for (m = 0;m<4;m++) for (m = 0;m<4;m++)
{ /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */ { /* m=0:gps/qzs/sbs, 1:glo, 2:gal, 3:bds */
nofix = (m == 1 && rtk->opt.glomodear == 0) || (m == 3 && rtk->opt.bdsmodear == 0); nofix = (m == 1 && rtk->opt.glomodear == 0) || (m == 3 && rtk->opt.bdsmodear == 0);
for (f = 0, k = na;f<nf;f++, k+=MAXSAT) for (f = 0, k = na;f<nf;f++, k+=MAXSAT)
{ {
for (i = k;i<k+MAXSAT;i++) for (i = k;i<k+MAXSAT;i++)
{ {
#if 0 #if 0
@ -1489,7 +1487,7 @@ int ddmat(rtk_t *rtk, double *D)
/* restore single-differenced ambiguity --------------------------------------*/ /* restore single-differenced ambiguity --------------------------------------*/
void restamb(rtk_t *rtk, const double *bias, int nb, double *xa) void restamb(rtk_t *rtk, const double *bias, int nb __attribute((unused)), double *xa)
{ {
int i, n, m, f, index[MAXSAT], nv = 0, nf = NF_RTK(&rtk->opt); int i, n, m, f, index[MAXSAT], nv = 0, nf = NF_RTK(&rtk->opt);
@ -1500,7 +1498,6 @@ int ddmat(rtk_t *rtk, double *D)
for (m = 0;m<4;m++) for (f = 0;f<nf;f++) for (m = 0;m<4;m++) for (f = 0;f<nf;f++)
{ {
for (n = i = 0;i<MAXSAT;i++) for (n = i = 0;i<MAXSAT;i++)
{ {
if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2) if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2)
@ -1520,7 +1517,6 @@ int ddmat(rtk_t *rtk, double *D)
} }
} }
/* hold integer ambiguity ----------------------------------------------------*/ /* hold integer ambiguity ----------------------------------------------------*/
void holdamb(rtk_t *rtk, const double *xa) void holdamb(rtk_t *rtk, const double *xa)
{ {
@ -1533,7 +1529,6 @@ int ddmat(rtk_t *rtk, double *D)
for (m = 0;m<4;m++) for (f = 0;f<nf;f++) for (m = 0;m<4;m++) for (f = 0;f<nf;f++)
{ {
for (n = i = 0;i<MAXSAT;i++) for (n = i = 0;i<MAXSAT;i++)
{ {
if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2 || if (!test_sys(rtk->ssat[i].sys, m) || rtk->ssat[i].fix[f] != 2 ||
@ -1545,7 +1540,8 @@ int ddmat(rtk_t *rtk, double *D)
rtk->ssat[i].fix[f] = 3; /* hold */ rtk->ssat[i].fix[f] = 3; /* hold */
} }
/* constraint to fixed ambiguity */ /* constraint to fixed ambiguity */
for (i = 1;i<n;i++) { for (i = 1;i<n;i++)
{
v[nv] = (xa[index[0]]-xa[index[i]])-(rtk->x[index[0]]-rtk->x[index[i]]); v[nv] = (xa[index[0]]-xa[index[i]])-(rtk->x[index[0]]-rtk->x[index[i]]);
H[index[0]+nv*rtk->nx] = 1.0; H[index[0]+nv*rtk->nx] = 1.0;
@ -1568,7 +1564,6 @@ int ddmat(rtk_t *rtk, double *D)
free(v); free(H); free(v); free(H);
} }
/* resolve integer ambiguity by LAMBDA ---------------------------------------*/ /* resolve integer ambiguity by LAMBDA ---------------------------------------*/
int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa) int resamb_LAMBDA(rtk_t *rtk, double *bias, double *xa)
{ {
@ -1610,7 +1605,6 @@ int ddmat(rtk_t *rtk, double *D)
/* lambda/mlambda integer least-square estimation */ /* lambda/mlambda integer least-square estimation */
if (!(info = lambda(nb, 2, y+na, Qb, b, s))) if (!(info = lambda(nb, 2, y+na, Qb, b, s)))
{ {
trace(4, "N(1)="); tracemat(4, b , 1, nb, 10, 3); trace(4, "N(1)="); tracemat(4, b , 1, nb, 10, 3);
trace(4, "N(2)="); tracemat(4, b+nb, 1, nb, 10, 3); trace(4, "N(2)="); tracemat(4, b+nb, 1, nb, 10, 3);
@ -1665,7 +1659,6 @@ int ddmat(rtk_t *rtk, double *D)
return nb; /* number of ambiguities */ return nb; /* number of ambiguities */
} }
/* validation of solution ----------------------------------------------------*/ /* validation of solution ----------------------------------------------------*/
int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg, int valpos(rtk_t *rtk, const double *v, const double *R, const int *vflg,
int nv, double thres) int nv, double thres)
@ -1695,7 +1688,6 @@ int ddmat(rtk_t *rtk, double *D)
#if 0 /* omitted v.2.4.0 */ #if 0 /* omitted v.2.4.0 */
if (stat&&nv>NP(opt)) if (stat&&nv>NP(opt))
{ {
/* chi-square validation */ /* chi-square validation */
for (i = 0; i<nv; i++) vv += v[i]*v[i]/R[i+i*nv]; for (i = 0; i<nv; i++) vv += v[i]*v[i]/R[i+i*nv];
@ -1715,7 +1707,6 @@ int ddmat(rtk_t *rtk, double *D)
return stat; return stat;
} }
/* relative positioning ------------------------------------------------------*/ /* relative positioning ------------------------------------------------------*/
int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr, int relpos(rtk_t *rtk, const obsd_t *obs, int nu, int nr,
const nav_t *nav) const nav_t *nav)
@ -1807,14 +1798,12 @@ int ddmat(rtk_t *rtk, double *D)
} }
if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel)) if (stat != SOLQ_NONE && zdres(0, obs, nu, rs, dts, svh, nav, xp, opt, 0, y, e, azel))
{ {
/* post-fit residuals for float solution */ /* post-fit residuals for float solution */
nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg); nv = ddres(rtk, nav, dt, xp, Pp, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg);
/* validation of float solution */ /* validation of float solution */
if (valpos(rtk, v, R, vflg, nv, 4.0)) if (valpos(rtk, v, R, vflg, nv, 4.0))
{ {
/* update state and covariance matrix */ /* update state and covariance matrix */
matcpy(rtk->x, xp, rtk->nx, 1); matcpy(rtk->x, xp, rtk->nx, 1);
matcpy(rtk->P, Pp, rtk->nx, rtk->nx); matcpy(rtk->P, Pp, rtk->nx, rtk->nx);
@ -1852,17 +1841,14 @@ int ddmat(rtk_t *rtk, double *D)
/* resolve integer ambiguity by LAMBDA */ /* resolve integer ambiguity by LAMBDA */
else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa)>1) else if (stat != SOLQ_NONE && resamb_LAMBDA(rtk, bias, xa)>1)
{ {
if (zdres(0, obs, nu, rs, dts, svh, nav, xa, opt, 0, y, e, azel)) if (zdres(0, obs, nu, rs, dts, svh, nav, xa, opt, 0, y, e, azel))
{ {
/* post-fit reisiduals for fixed solution */ /* post-fit reisiduals for fixed solution */
nv = ddres(rtk, nav, dt, xa, NULL, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg); nv = ddres(rtk, nav, dt, xa, NULL, sat, y, e, azel, iu, ir, ns, v, NULL, R, vflg);
/* validation of fixed solution */ /* validation of fixed solution */
if (valpos(rtk, v, R, vflg, nv, 4.0)) if (valpos(rtk, v, R, vflg, nv, 4.0))
{ {
/* hold integer ambiguity */ /* hold integer ambiguity */
if (++rtk->nfix >= rtk->opt.minfix && if (++rtk->nfix >= rtk->opt.minfix &&
rtk->opt.modear == ARMODE_FIXHOLD) rtk->opt.modear == ARMODE_FIXHOLD)
@ -1873,6 +1859,7 @@ int ddmat(rtk_t *rtk, double *D)
} }
} }
} }
/* save solution status */ /* save solution status */
if (stat == SOLQ_FIX) if (stat == SOLQ_FIX)
{ {
@ -1905,7 +1892,6 @@ int ddmat(rtk_t *rtk, double *D)
} }
for (i = 0;i<ns;i++) for (j = 0;j<nf;j++) for (i = 0;i<ns;i++) for (j = 0;j<nf;j++)
{ {
/* output snr of rover receiver */ /* output snr of rover receiver */
rtk->ssat[sat[i]-1].snr[j] = obs[iu[i]].SNR[j]; rtk->ssat[sat[i]-1].snr[j] = obs[iu[i]].SNR[j];
} }
@ -1922,7 +1908,6 @@ int ddmat(rtk_t *rtk, double *D)
return stat != SOLQ_NONE; return stat != SOLQ_NONE;
} }
/* initialize rtk control ------------------------------------------------------ /* initialize rtk control ------------------------------------------------------
* initialize rtk control struct * initialize rtk control struct
* args : rtk_t *rtk IO rtk control/result struct * args : rtk_t *rtk IO rtk control/result struct

12
src/algorithms/libs/rtklib/rtklib_rtkpos.h
View File

@ -90,12 +90,12 @@ const double TTOL_MOVEB = (1.0+2*DTTOL);
#define IB_RTK(s,f,opt) (NR_RTK(opt)+MAXSAT*(f)+(s)-1) /* phase bias (s:satno,f:freq) */ #define IB_RTK(s,f,opt) (NR_RTK(opt)+MAXSAT*(f)+(s)-1) /* phase bias (s:satno,f:freq) */
static int resamb_WLNL(rtk_t *rtk, const obsd_t *obs, const int *sat, static int resamb_WLNL(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu, const int *ir, int ns, const nav_t *nav, const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns, const nav_t *nav __attribute((unused)),
const double *azel) {return 0;} const double *azel __attribute((unused))) {return 0;}
static int resamb_TCAR(rtk_t *rtk, const obsd_t *obs, const int *sat, static int resamb_TCAR(rtk_t *rtk __attribute((unused)), const obsd_t *obs __attribute((unused)), const int *sat __attribute((unused)),
const int *iu, const int *ir, int ns, const nav_t *nav, const int *iu __attribute((unused)), const int *ir __attribute((unused)), int ns __attribute((unused)), const nav_t *nav __attribute((unused)),
const double *azel) {return 0;} const double *azel __attribute((unused))) {return 0;}
/* global variables ----------------------------------------------------------*/ /* global variables ----------------------------------------------------------*/
static int statlevel = 0; /* rtk status output level (0:off) */ static int statlevel = 0; /* rtk status output level (0:off) */