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

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

@ -183,101 +183,113 @@ void rtkclosestat(void)
/* write solution status to buffer -------------------------------------------*/ /* write solution status to buffer -------------------------------------------*/
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 { }
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++)
if (!ssat->vs) continue; {
satno2id(i+1,id); ssat = rtk->ssat+i;
j=II_RTK(i+1,&rtk->opt); if (!ssat->vs) continue;
xa[0]=j<rtk->na?rtk->xa[j]:0.0; satno2id(i+1, id);
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat, j = II_RTK(i+1, &rtk->opt);
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[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,
} 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++) { /* tropospheric parameters */
j=IT_RTK(i,&rtk->opt); if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG))
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, for (i = 0;i<2;i++)
i+1,rtk->x[j],xa[0]); {
} j = IT_RTK(i, &rtk->opt);
} xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
/* receiver h/w bias */ fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
if (est&&rtk->opt.glomodear==2) { i+1, rtk->x[j], xa[0]);
for (i=0;i<nfreq;i++) { }
j=IL_RTK(i,&rtk->opt); }
xa[0]=j<rtk->na?rtk->xa[j]:0.0; /* receiver h/w bias */
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, if (est && rtk->opt.glomodear == 2)
i+1,rtk->x[j],xa[0]); {
} for (i = 0;i<nfreq;i++)
} {
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; 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; {
if (!ssat->vs) continue; ssat = rtk->ssat+i;
satno2id(i+1,id); if (!ssat->vs) continue;
for (j=0;j<nfreq;j++) { satno2id(i+1, id);
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", for (j = 0;j<nfreq;j++)
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, {
ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25, fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j], week, tow, id, j+1, ssat->azel[0]*R2D, ssat->azel[1]*R2D,
ssat->slipc[j],ssat->rejc[j]); ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25,
} ssat->fix [j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j],
} ssat->slipc[j], ssat->rejc[j]);
}
}
} }
@ -312,101 +324,113 @@ void swapsolstat(void)
/* output solution status ----------------------------------------------------*/ /* output solution status ----------------------------------------------------*/
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++)
if (!ssat->vs) continue; {
satno2id(i+1,id); ssat = rtk->ssat+i;
j=II_RTK(i+1,&rtk->opt); if (!ssat->vs) continue;
xa[0]=j<rtk->na?rtk->xa[j]:0.0; satno2id(i+1, id);
fprintf(fp_stat,"$ION,%d,%.3f,%d,%s,%.1f,%.1f,%.4f,%.4f\n",week,tow,rtk->sol.stat, j = II_RTK(i+1, &rtk->opt);
id,ssat->azel[0]*R2D,ssat->azel[1]*R2D,rtk->x[j],xa[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,
} 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++) { /* tropospheric parameters */
j=IT_RTK(i,&rtk->opt); if (est && (rtk->opt.tropopt == TROPOPT_EST || rtk->opt.tropopt == TROPOPT_ESTG))
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, for (i = 0;i<2;i++)
i+1,rtk->x[j],xa[0]); {
} j = IT_RTK(i, &rtk->opt);
} xa[0] = j < rtk->na ? rtk->xa[j] : 0.0;
/* receiver h/w bias */ fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
if (est&&rtk->opt.glomodear==2) { i+1, rtk->x[j], xa[0]);
for (i=0;i<nfreq;i++) { }
j=IL_RTK(i,&rtk->opt); }
xa[0]=j<rtk->na?rtk->xa[j]:0.0; /* receiver h/w bias */
fprintf(fp_stat,"$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n",week,tow,rtk->sol.stat, if (est && rtk->opt.glomodear == 2)
i+1,rtk->x[j],xa[0]); {
} for (i = 0;i<nfreq;i++)
} {
if (rtk->sol.stat==SOLQ_NONE||statlevel<=1) return; 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; {
if (!ssat->vs) continue; ssat = rtk->ssat+i;
satno2id(i+1,id); if (!ssat->vs) continue;
for (j=0;j<nfreq;j++) { satno2id(i+1, id);
fprintf(fp_stat,"$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n", for (j = 0;j<nfreq;j++)
week,tow,id,j+1,ssat->azel[0]*R2D,ssat->azel[1]*R2D, {
ssat->resp [j],ssat->resc[j], ssat->vsat[j],ssat->snr[j]*0.25, fprintf(fp_stat, "$SAT,%d,%.3f,%s,%d,%.1f,%.1f,%.4f,%.4f,%d,%.0f,%d,%d,%d,%d,%d,%d\n",
ssat->fix [j],ssat->slip[j]&3,ssat->lock[j],ssat->outc[j], week, tow, id, j+1, ssat->azel[0]*R2D, ssat->azel[1]*R2D,
ssat->slipc[j],ssat->rejc[j]); ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25,
} ssat->fix [j], ssat->slip[j]&3, ssat->lock[j], ssat->outc[j],
} ssat->slipc[j], ssat->rejc[j]);
}
}
} }