From 847db9ea10fba77a2de7d5ab519f70544b2c2b9a Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Sat, 6 May 2017 13:00:13 +0200 Subject: [PATCH] Make code more readable --- src/algorithms/libs/rtklib/rtklib_rtkpos.cc | 376 +++++++++++--------- 1 file changed, 200 insertions(+), 176 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc index 27f57ed4a..11a117b80 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkpos.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkpos.cc @@ -183,101 +183,113 @@ void rtkclosestat(void) /* write solution status to buffer -------------------------------------------*/ void rtkoutstat(rtk_t *rtk, char *buff) { - ssat_t *ssat; - 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); - char id[32]; + ssat_t *ssat; + 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); + 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 */ - swapsolstat(); + /* swap solution status file */ + swapsolstat(); - est=rtk->opt.mode>=PMODE_DGPS; - nfreq=est?nf:1; - tow=time2gpst(rtk->sol.time,&week); + est = rtk->opt.mode >= PMODE_DGPS; + nfreq = est ? nf : 1; + tow = time2gpst(rtk->sol.time, &week); - /* receiver position */ - if (est) { - for (i=0;i<3;i++) xa[i]=ina?rtk->xa[i]:0.0; - 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, - 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 */ - if (est&&rtk->opt.dynamics) { - ecef2pos(rtk->sol.rr,pos); - ecef2enu(pos,rtk->x+3,vel); - ecef2enu(pos,rtk->x+6,acc); - if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela); - 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", - 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 { - ecef2pos(rtk->sol.rr,pos); - 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", - 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 */ - 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); + /* receiver position */ + 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, + 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, + 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 */ + if (est && rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->x+3, vel); + ecef2enu(pos, rtk->x+6, acc); + if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela); + 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", + 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 + { + ecef2pos(rtk->sol.rr, pos); + 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", + 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 */ + 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 */ - if (est&&rtk->opt.ionoopt==IONOOPT_EST) { - for (i=0;issat+i; - if (!ssat->vs) continue; - satno2id(i+1,id); - j=II_RTK(i+1,&rtk->opt); - xa[0]=jna?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++) { - j=IT_RTK(i,&rtk->opt); - xa[0]=jna?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 */ - if (est&&rtk->opt.glomodear==2) { - for (i=0;iopt); - xa[0]=jna?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; + /* ionospheric parameters */ + if (est && rtk->opt.ionoopt == IONOOPT_EST) + { + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + j = II_RTK(i+1, &rtk->opt); + 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++) + { + 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]); + } + } + /* receiver h/w bias */ + if (est && rtk->opt.glomodear == 2) + { + for (i = 0;iopt); + 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 */ - for (i=0;issat+i; - if (!ssat->vs) continue; - satno2id(i+1,id); - for (j=0;jazel[0]*R2D,ssat->azel[1]*R2D, - 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]); - } - } + /* residuals and status */ + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + for (j = 0;jazel[0]*R2D, ssat->azel[1]*R2D, + 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 ----------------------------------------------------*/ void outsolstat(rtk_t *rtk) { - ssat_t *ssat; -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); -char id[32]; + ssat_t *ssat; + 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); + 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 */ -swapsolstat(); + /* swap solution status file */ + swapsolstat(); -est=rtk->opt.mode >= PMODE_DGPS; -nfreq=est?nf:1; -tow=time2gpst(rtk->sol.time,&week); + est = rtk->opt.mode >= PMODE_DGPS; + nfreq = est ? nf : 1; + tow = time2gpst(rtk->sol.time, &week); -/* receiver position */ -if (est) { - for (i=0;i<3;i++) xa[i]=ina?rtk->xa[i]:0.0; - 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, - 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 */ -if (est&&rtk->opt.dynamics) { - ecef2pos(rtk->sol.rr,pos); - ecef2enu(pos,rtk->x+3,vel); - ecef2enu(pos,rtk->x+6,acc); - if (rtk->na>=6) ecef2enu(pos,rtk->xa+3,vela); - 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", - 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 { - ecef2pos(rtk->sol.rr,pos); - 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", - 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 */ -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); + /* receiver position */ + 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, + 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, + 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 */ + if (est && rtk->opt.dynamics) + { + ecef2pos(rtk->sol.rr, pos); + ecef2enu(pos, rtk->x+3, vel); + ecef2enu(pos, rtk->x+6, acc); + if (rtk->na >= 6) ecef2enu(pos, rtk->xa+3, vela); + 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", + 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 + { + ecef2pos(rtk->sol.rr, pos); + 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", + 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 */ + 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 */ -if (est&&rtk->opt.ionoopt==IONOOPT_EST) { - for (i=0;issat+i; - if (!ssat->vs) continue; - satno2id(i+1,id); - j=II_RTK(i+1,&rtk->opt); - xa[0]=jna?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++) { - j=IT_RTK(i,&rtk->opt); - xa[0]=jna?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 */ -if (est&&rtk->opt.glomodear==2) { - for (i=0;iopt); - xa[0]=jna?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; + /* ionospheric parameters */ + if (est && rtk->opt.ionoopt == IONOOPT_EST) + { + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + j = II_RTK(i+1, &rtk->opt); + 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++) + { + 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]); + } + } + /* receiver h/w bias */ + if (est && rtk->opt.glomodear == 2) + { + for (i = 0;iopt); + 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 */ -for (i=0;issat+i; - if (!ssat->vs) continue; - satno2id(i+1,id); - for (j=0;jazel[0]*R2D,ssat->azel[1]*R2D, - 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]); - } -} + /* residuals and status */ + for (i = 0;issat+i; + if (!ssat->vs) continue; + satno2id(i+1, id); + for (j = 0;jazel[0]*R2D, ssat->azel[1]*R2D, + 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]); + } + } }