mirrors/gnss-sdr
1
0
Fork 0
mirror of https://github.com/gnss-sdr/gnss-sdr synced 2024-12-14 20:20:35 +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

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

@ -200,18 +200,21 @@ void rtkoutstat(rtk_t *rtk, char *buff)
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; 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, 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]); 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, 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], rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2],
0.0, 0.0, 0.0); 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); ecef2pos(rtk->sol.rr, pos);
ecef2enu(pos, rtk->x+3, vel); ecef2enu(pos, rtk->x+3, vel);
ecef2enu(pos, rtk->x+6, acc); ecef2enu(pos, rtk->x+6, acc);
@ -221,7 +224,8 @@ void rtkoutstat(rtk_t *rtk, char *buff)
week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[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]); vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]);
} }
else { else
{
ecef2pos(rtk->sol.rr, pos); ecef2pos(rtk->sol.rr, pos);
ecef2enu(pos, rtk->sol.rr+3, vel); 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", fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
@ -234,8 +238,10 @@ void rtkoutstat(rtk_t *rtk, char *buff)
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++) { {
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);
@ -246,8 +252,10 @@ void rtkoutstat(rtk_t *rtk, char *buff)
} }
} }
/* 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++) { {
for (i = 0;i<2;i++)
{
j = IT_RTK(i, &rtk->opt); j = IT_RTK(i, &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, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
@ -255,8 +263,10 @@ void rtkoutstat(rtk_t *rtk, char *buff)
} }
} }
/* 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++) { {
for (i = 0;i<nfreq;i++)
{
j = IL_RTK(i, &rtk->opt); j = IL_RTK(i, &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, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
@ -266,11 +276,13 @@ void rtkoutstat(rtk_t *rtk, char *buff)
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", 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, 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, ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25,
@ -329,18 +341,21 @@ 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; 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, 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]); 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, 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], rtk->sol.stat, rtk->sol.rr[0], rtk->sol.rr[1], rtk->sol.rr[2],
0.0, 0.0, 0.0); 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); ecef2pos(rtk->sol.rr, pos);
ecef2enu(pos, rtk->x+3, vel); ecef2enu(pos, rtk->x+3, vel);
ecef2enu(pos, rtk->x+6, acc); ecef2enu(pos, rtk->x+6, acc);
@ -350,7 +365,8 @@ if (est&&rtk->opt.dynamics) {
week, tow, rtk->sol.stat, vel[0], vel[1], vel[2], acc[0], acc[1], acc[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]); vela[0], vela[1], vela[2], acca[0], acca[1], acca[2]);
} }
else { else
{
ecef2pos(rtk->sol.rr, pos); ecef2pos(rtk->sol.rr, pos);
ecef2enu(pos, rtk->sol.rr+3, vel); 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", fprintf(fp_stat, "$VELACC,%d,%.3f,%d,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f,%.4f,%.4f,%.4f,%.5f,%.5f,%.5f\n",
@ -363,8 +379,10 @@ fprintf(fp_stat,"$CLK,%d,%.3f,%d,%d,%.3f,%.3f,%.3f,%.3f\n",
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++) { {
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);
@ -375,8 +393,10 @@ if (est&&rtk->opt.ionoopt==IONOOPT_EST) {
} }
} }
/* 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++) { {
for (i = 0;i<2;i++)
{
j = IT_RTK(i, &rtk->opt); j = IT_RTK(i, &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, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, fprintf(fp_stat, "$TROP,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
@ -384,8 +404,10 @@ if (est&&(rtk->opt.tropopt==TROPOPT_EST||rtk->opt.tropopt==TROPOPT_ESTG)) {
} }
} }
/* 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++) { {
for (i = 0;i<nfreq;i++)
{
j = IL_RTK(i, &rtk->opt); j = IL_RTK(i, &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, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat, fprintf(fp_stat, "$HWBIAS,%d,%.3f,%d,%d,%.4f,%.4f\n", week, tow, rtk->sol.stat,
@ -395,11 +417,13 @@ if (est&&rtk->opt.glomodear==2) {
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", 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, 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, ssat->resp [j], ssat->resc[j], ssat->vsat[j], ssat->snr[j]*0.25,