mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 12:10:34 +00:00
Code Issues Releases Wiki Activity

Make code more readable

Browse Source
This commit is contained in:
Carles Fernandez 2017-05-06 13:00:13 +02:00
parent f49e5ae3b0
commit 847db9ea10
1 changed files with 200 additions and 176 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -184,98 +184,110 @@ void rtkclosestat(void)
void rtkoutstat(rtk_t *rtk, char *buff) void rtkoutstat(rtk_t *rtk, char *buff)
{ {
ssat_t *ssat; ssat_t *ssat;
double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3]; double tow, pos[3], vel[3], acc[3], vela[3] = {0}, acca[3] = {0}, xa[3];
int i,j,week,est,nfreq,nf=NF_RTK(&rtk->opt); int i, j, week, est, nfreq, nf = NF_RTK(&rtk->opt);
char id[32]; char id[32];
if (statlevel<=0||!fp_stat) return; if (statlevel <= 0 || !fp_stat) return;
trace(3,"outsolstat:\n"); trace(3, "outsolstat:\n");
/* swap solution status file */ /* swap solution status file */
swapsolstat(); swapsolstat();
est=rtk->opt.mode>=PMODE_DGPS; est = rtk->opt.mode >= PMODE_DGPS;
nfreq=est?nf:1; nfreq = est ? nf : 1;
tow=time2gpst(rtk->sol.time,&week); tow = time2gpst(rtk->sol.time, &week);
/* receiver position */ /* receiver position */
if (est) { if (est)
for (i=0;i<3;i++) xa[i]=i<rtk->na?rtk->xa[i]:0.0; {
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, for (i = 0;i<3;i++) xa[i] = i < rtk->na ? rtk->xa[i] : 0.0;
rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]); fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow,
rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], xa[0], xa[1], xa[2]);
} }
else { else
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, {
rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2], fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow,
0.0,0.0,0.0); rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2],
0.0, 0.0, 0.0);
} }
/* receiver velocity and acceleration */ /* receiver velocity and acceleration */
if (est&&rtk->opt.dynamics) { if (est && rtk->opt.dynamics)
ecef2pos(rtk->sol.rr,pos); {
ecef2enu(pos,rtk->x+3,vel); ecef2pos(rtk->sol.rr, pos);
ecef2enu(pos,rtk->x+6,acc); ecef2enu(pos, rtk->x+3, vel);
if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela); ecef2enu(pos, rtk->x+6, acc);
if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca); if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", if (rtk->na >= 9) ecef2enu(pos, rtk->xa+6, acca);
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2], fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]); week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[2],
vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]);
} }
else { else
ecef2pos(rtk->sol.rr,pos); {
ecef2enu(pos,rtk->sol.rr+3,vel); ecef2pos(rtk->sol.rr, pos);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", ecef2enu(pos, rtk->sol.rr+3, vel);
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2], fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); week, tow, rtk->sol.stat, vel[0], vel[1], vel[2],
0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
} }
/* receiver clocks */ /* receiver clocks */
fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", fprintf(fp_stat, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9, week, tow, rtk->sol.stat, 1, rtk->sol.dtr[0]*1E9, rtk->sol.dtr[1]*1E9,
rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9); rtk->sol.dtr[2]*1E9, rtk->sol.dtr[3]*1E9);
/* ionospheric parameters */ /* ionospheric parameters */
if (est&&rtk->opt.ionoopt==IONOOPT_EST) { if (est && rtk->opt.ionoopt == IONOOPT_EST)
for (i=0;i<MAXSAT;i++) { {
ssat=rtk->ssat+i; for (i = 0;i<MAXSAT;i++)
{
ssat = rtk->ssat+i;
if (!ssat->vs) continue; if (!ssat->vs) continue;
satno2id(i+1,id); satno2id(i+1, id);
j=II_RTK(i+1,&rtk->opt); j = II_RTK(i+1, &rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0; xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat, fprintf(fp_stat, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, rtk->sol.stat,
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]); id, ssat->azel[0]*R2D, ssat->azel[1]*R2D, rtk->x[j], xa[0]);
} }
} }
/* tropospheric parameters */ /* tropospheric parameters */
if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) { if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG))
for (i=0;i<2;i++) { {
j=IT_RTK(i,&rtk->opt); for (i = 0;i<2;i++)
xa[0]=j<rtk->na?rtk->xa[j]:0.0; {
fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, j = IT_RTK(i, &rtk->opt);
i+1,rtk->x[j],xa[0]); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
i+1, rtk->x[j], xa[0]);
} }
} }
/* receiver h/w bias */ /* receiver h/w bias */
if (est&&rtk->opt.glomodear==2) { if (est && rtk->opt.glomodear == 2)
for (i=0;i<nfreq;i++) { {
j=IL_RTK(i,&rtk->opt); for (i = 0;i<nfreq;i++)
xa[0]=j<rtk->na?rtk->xa[j]:0.0; {
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, j = IL_RTK(i, &rtk->opt);
i+1,rtk->x[j],xa[0]); xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
i+1, rtk->x[j], xa[0]);
} }
} }
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return;
/* residuals and status */ /* residuals and status */
for (i=0;i<MAXSAT;i++) { for (i = 0;i<MAXSAT;i++)
ssat=rtk->ssat+i; {
ssat = rtk->ssat+i;
if (!ssat->vs) continue; if (!ssat->vs) continue;
satno2id(i+1,id); satno2id(i+1, id);
for (j=0;j<nfreq;j++) { for (j = 0;j<nfreq;j++)
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", {
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25, week, tow, id, j+1, ssat->azel[0]*R2D, ssat->azel[1]*R2D,
ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j], ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25,
ssat->slipc[j],ssat->rejc[j]); ssat->fix [j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j],
ssat->slipc[j], ssat->rejc[j]);
} }
} }
} }
@ -313,100 +325,112 @@ void swapsolstat(void)
void outsolstat(rtk_t *rtk) void outsolstat(rtk_t *rtk)
{ {
ssat_t *ssat; ssat_t *ssat;
double tow,pos[3],vel[3],acc[3],vela[3]={0},acca[3]={0},xa[3]; double tow, pos[3], vel[3], acc[3], vela[3] = {0}, acca[3] = {0}, xa[3];
int i,j,week,est,nfreq,nf=NF_RTK(&rtk->opt); int i, j, week, est, nfreq, nf = NF_RTK(&rtk->opt);
char id[32]; char id[32];
if (statlevel<=0||!fp_stat) return; if (statlevel <= 0 || !fp_stat) return;
trace(3,"outsolstat:\n"); trace(3, "outsolstat:\n");
/* swap solution status file */ /* swap solution status file */
swapsolstat(); swapsolstat();
est=rtk->opt.mode >= PMODE_DGPS; est = rtk->opt.mode >= PMODE_DGPS;
nfreq=est?nf:1; nfreq = est ? nf : 1;
tow=time2gpst(rtk->sol.time,&week); tow = time2gpst(rtk->sol.time, &week);
/* receiver position */ /* receiver position */
if (est) { if (est)
for (i=0;i<3;i++) xa[i]=i<rtk->na?rtk->xa[i]:0.0; {
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, for (i = 0;i<3;i++) xa[i] = i < rtk->na ? rtk->xa[i] : 0.0;
rtk->sol.stat,rtk->x[0],rtk->x[1],rtk->x[2],xa[0],xa[1],xa[2]); fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow,
} rtk->sol.stat, rtk->x[0], rtk->x[1], rtk->x[2], xa[0], xa[1], xa[2]);
else { }
fprintf(fp_stat,"$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n",week,tow, else
rtk->sol.stat,rtk->sol.rr[0],rtk->sol.rr[1],rtk->sol.rr[2], {
0.0,0.0,0.0); fprintf(fp_stat, "$POS,%d,%.3f,%d,%.4f,%.4f,%.4f,%.4f,%.4f,%.4f\n", week, tow,
} rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2],
/* receiver velocity and acceleration */ 0.0, 0.0, 0.0);
if (est&&rtk->opt.dynamics) { }
ecef2pos(rtk->sol.rr,pos); /* receiver velocity and acceleration */
ecef2enu(pos,rtk->x+3,vel); if (est && rtk->opt.dynamics)
ecef2enu(pos,rtk->x+6,acc); {
if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela); ecef2pos(rtk->sol.rr, pos);
if (rtk->na>=9) ecef2enu(pos,rtk->xa+6,acca); ecef2enu(pos, rtk->x+3, vel);
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", ecef2enu(pos, rtk->x+6, acc);
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2],acc[0],acc[1],acc[2], if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela);
vela[0],vela[1],vela[2],acca[0],acca[1],acca[2]); if (rtk->na >= 9) ecef2enu(pos, rtk->xa+6, acca);
} fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
else { week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[2],
ecef2pos(rtk->sol.rr,pos); vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]);
ecef2enu(pos,rtk->sol.rr+3,vel); }
fprintf(fp_stat,"$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n", else
week,tow,rtk->sol.stat,vel[0],vel[1],vel[2], {
0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0,0.0); ecef2pos(rtk->sol.rr, pos);
} ecef2enu(pos, rtk->sol.rr+3, vel);
/* receiver clocks */ fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n", week, tow, rtk->sol.stat, vel[0], vel[1], vel[2],
week,tow,rtk->sol.stat,1,rtk->sol.dtr[0]*1E9,rtk->sol.dtr[1]*1E9, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0);
rtk->sol.dtr[2]*1E9,rtk->sol.dtr[3]*1E9); }
/* receiver clocks */
fprintf(fp_stat, "$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
week, tow, rtk->sol.stat, 1, rtk->sol.dtr[0]*1E9, rtk->sol.dtr[1]*1E9,
rtk->sol.dtr[2]*1E9, rtk->sol.dtr[3]*1E9);
/* ionospheric parameters */ /* ionospheric parameters */
if (est&&rtk->opt.ionoopt==IONOOPT_EST) { if (est && rtk->opt.ionoopt == IONOOPT_EST)
for (i=0;i<MAXSAT;i++) { {
ssat=rtk->ssat+i; for (i = 0;i<MAXSAT;i++)
{
ssat = rtk->ssat+i;
if (!ssat->vs) continue; if (!ssat->vs) continue;
satno2id(i+1,id); satno2id(i+1, id);
j=II_RTK(i+1,&rtk->opt); j = II_RTK(i+1, &rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0; xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat, fprintf(fp_stat, "$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n", week, tow, rtk->sol.stat,
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[0]); id, ssat->azel[0]*R2D, ssat->azel[1]*R2D, rtk->x[j], xa[0]);
} }
}
/* tropospheric parameters */
if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) {
for (i=0;i<2;i++) {
j=IT_RTK(i,&rtk->opt);
xa[0]=j<rtk->na?rtk->xa[j]:0.0;
fprintf(fp_stat,"$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat,
i+1,rtk->x[j],xa[0]);
} }
} /* tropospheric parameters */
/* receiver h/w bias */ if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG))
if (est&&rtk->opt.glomodear==2) { {
for (i=0;i<nfreq;i++) { for (i = 0;i<2;i++)
j=IL_RTK(i,&rtk->opt); {
xa[0]=j<rtk->na?rtk->xa[j]:0.0; j = IT_RTK(i, &rtk->opt);
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
i+1,rtk->x[j],xa[0]); fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
i+1, rtk->x[j], xa[0]);
} }
} }
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; /* receiver h/w bias */
if (est && rtk->opt.glomodear == 2)
{
for (i = 0;i<nfreq;i++)
{
j = IL_RTK(i, &rtk->opt);
xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
i+1, rtk->x[j], xa[0]);
}
}
if (rtk->sol.stat == SOLQ_NONE || statlevel <= 1) return;
/* residuals and status */ /* residuals and status */
for (i=0;i<MAXSAT;i++) { for (i = 0;i<MAXSAT;i++)
ssat=rtk->ssat+i; {
ssat = rtk->ssat+i;
if (!ssat->vs) continue; if (!ssat->vs) continue;
satno2id(i+1,id); satno2id(i+1, id);
for (j=0;j<nfreq;j++) { for (j = 0;j<nfreq;j++)
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", {
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25, week, tow, id, j+1, ssat->azel[0]*R2D, ssat->azel[1]*R2D,
ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j], ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25,
ssat->slipc[j],ssat->rejc[j]); ssat->fix [j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j],
ssat->slipc[j], ssat->rejc[j]);
}
} }
}
} }