From 9da436ec870b480352ad068886e20fd42eaf60e2 Mon Sep 17 00:00:00 2001 From: Carles Fernandez Date: Fri, 12 May 2017 17:47:09 +0200 Subject: [PATCH] add more work on rtklib functions --- src/algorithms/libs/rtklib/rtklib.h | 1 - src/algorithms/libs/rtklib/rtklib_rtkcmn.cc | 9 +- src/algorithms/libs/rtklib/rtklib_rtkcmn.h | 3 +- src/algorithms/libs/rtklib/rtklib_rtksvr.cc | 527 ++++---- src/algorithms/libs/rtklib/rtklib_solution.cc | 1077 +++++++++-------- src/algorithms/libs/rtklib/rtklib_solution.h | 3 + src/algorithms/libs/rtklib/rtklib_stream.cc | 218 ++-- 7 files changed, 926 insertions(+), 912 deletions(-) diff --git a/src/algorithms/libs/rtklib/rtklib.h b/src/algorithms/libs/rtklib/rtklib.h index bfc6a9310..33b7a16d9 100644 --- a/src/algorithms/libs/rtklib/rtklib.h +++ b/src/algorithms/libs/rtklib/rtklib.h @@ -66,7 +66,6 @@ const int FILEPATHSEP = '/'; - const double RE_WGS84 = 6378137.0; //!< earth semimajor axis (WGS84) (m) const double FE_WGS84 = (1.0 / 298.257223563); //!< earth flattening (WGS84) diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc index 7cb0f9e97..2918cd8cc 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.cc @@ -65,7 +65,7 @@ const double gpst0[] = {1980, 1, 6, 0, 0, 0}; /* gps time reference */ const double gst0 [] = {1999, 8, 22, 0, 0, 0}; /* galileo system time reference */ const double bdt0 [] = {2006, 1, 1, 0, 0, 0}; /* beidou time reference */ -static double timeoffset_ = 0.0; +static double timeoffset_ = 0.0; double leaps[MAXLEAPS+1][7] = { /* leap seconds (y,m,d,h,m,s,utc-gpst) */ {2017, 1, 1, 0, 0, 0, -18}, @@ -1703,6 +1703,13 @@ void deg2dms(double deg, double *dms, int ndec) dms[0]*=sign; } +void deg2dms(double deg, double *dms) +{ + double sign=deg<0.0?-1.0:1.0,a=fabs(deg); + dms[0]=floor(a); a=(a-dms[0])*60.0; + dms[1]=floor(a); a=(a-dms[1])*60.0; + dms[2]=a; dms[0]*=sign; +} /* convert deg-min-sec to degree ----------------------------------------------- * convert degree-minute-second to degree diff --git a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h index c5956296a..9b2b15027 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtkcmn.h +++ b/src/algorithms/libs/rtklib/rtklib_rtkcmn.h @@ -174,6 +174,7 @@ int adjgpsweek(int week); unsigned int tickget(void); void sleepms(int ms); void deg2dms(double deg, double *dms, int ndec); +void deg2dms(double deg, double *dms); double dms2deg(const double *dms); void ecef2pos(const double *r, double *pos); void pos2ecef(const double *pos, double *r); @@ -182,8 +183,6 @@ void ecef2enu(const double *pos, const double *r, double *e); void enu2ecef(const double *pos, const double *e, double *r); void covenu(const double *pos, const double *P, double *Q); void covecef(const double *pos, const double *Q, double *P); - - void ast_args(double t, double *f); void nut_iau1980(double t, const double *f, double *dpsi, double *deps); void eci2ecef(gtime_t tutc, const double *erpv, double *U, double *gmst); diff --git a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc index ca9e7e8a7..328adffc6 100644 --- a/src/algorithms/libs/rtklib/rtklib_rtksvr.cc +++ b/src/algorithms/libs/rtklib/rtklib_rtksvr.cc @@ -6,14 +6,14 @@ #include "rtklib_sbas.h" #include "rtklib_preceph.h" #include "rtklib_stream.h" +#include "rtklib_rtcm.h" /* write solution header to output stream ------------------------------------*/ void writesolhead(stream_t *stream, const solopt_t *solopt) { unsigned char buff[1024]; int n; - - n=outsolheads(buff,solopt); + n = outsolheads(buff,solopt); strwrite(stream,buff,n); } @@ -23,9 +23,9 @@ void saveoutbuf(rtksvr_t *svr, unsigned char *buff, int n, int index) { rtksvrlock(svr); - n=nbuffsize-svr->nsb[index]?n:svr->buffsize-svr->nsb[index]; + n = nbuffsize-svr->nsb[index]?n:svr->buffsize-svr->nsb[index]; memcpy(svr->sbuf[index]+svr->nsb[index],buff,n); - svr->nsb[index]+=n; + svr->nsb[index] += n; rtksvrunlock(svr); } @@ -34,23 +34,23 @@ void saveoutbuf(rtksvr_t *svr, unsigned char *buff, int n, int index) /* write solution to output stream -------------------------------------------*/ void writesol(rtksvr_t *svr, int index) { - solopt_t solopt=solopt_default; + solopt_t solopt = solopt_default; unsigned char buff[1024]; int i,n; tracet(4,"writesol: index=%d\n",index); - for (i=0;i<2;i++) + for (i = 0;i<2;i++) { /* output solution */ - n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,svr->solopt+i); + n = outsols(buff,&svr->rtk.sol,svr->rtk.rb,svr->solopt+i); strwrite(svr->stream+i+3,buff,n); /* save output buffer */ saveoutbuf(svr,buff,n,i); /* output extended solution */ - n=outsolexs(buff,&svr->rtk.sol,svr->rtk.ssat,svr->solopt+i); + n = outsolexs(buff,&svr->rtk.sol,svr->rtk.ssat,svr->solopt+i); strwrite(svr->stream+i+3,buff,n); /* save output buffer */ @@ -59,14 +59,14 @@ void writesol(rtksvr_t *svr, int index) /* output solution to monitor port */ if (svr->moni) { - n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,&solopt); + n = outsols(buff,&svr->rtk.sol,svr->rtk.rb,&solopt); strwrite(svr->moni,buff,n); } /* save solution buffer */ if (svr->nsolsolbuf[svr->nsol++]=svr->rtk.sol; + svr->solbuf[svr->nsol++] = svr->rtk.sol; rtksvrunlock(svr); } } @@ -76,9 +76,9 @@ void writesol(rtksvr_t *svr, int index) void updatenav(nav_t *nav) { int i,j; - for (i=0;ilam[i][j]=satwavelen(i+1,j,nav); + nav->lam[i][j] = satwavelen(i+1,j,nav); } } @@ -88,22 +88,22 @@ void updatefcn(rtksvr_t *svr) { int i,j,sat,frq; - for (i=0;iraw[j].nav.geph[i].sat!=sat) continue; - frq=svr->raw[j].nav.geph[i].frq; + if (svr->raw[j].nav.geph[i].sat != sat) continue; + frq = svr->raw[j].nav.geph[i].frq; } - if (frq<-7||frq>6) continue; + if (frq<-7 || frq>6) continue; - for (j=0;j<3;j++) + for (j = 0;j<3;j++) { - if (svr->raw[j].nav.geph[i].sat==sat) continue; - svr->raw[j].nav.geph[i].sat=sat; - svr->raw[j].nav.geph[i].frq=frq; + if (svr->raw[j].nav.geph[i].sat == sat) continue; + svr->raw[j].nav.geph[i].sat = sat; + svr->raw[j].nav.geph[i].frq = frq; } } } @@ -116,43 +116,43 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat, eph_t *eph1,*eph2,*eph3; geph_t *geph1,*geph2,*geph3; gtime_t tof; - double pos[3],del[3]={0},dr[3]; - int i,n=0,prn,sbssat=svr->rtk.opt.sbassatsel,sys,iode; + double pos[3],del[3] = {0},dr[3]; + int i,n = 0,prn,sbssat = svr->rtk.opt.sbassatsel,sys,iode; tracet(4,"updatesvr: ret=%d sat=%2d index=%d\n",ret,sat,index); - if (ret==1) + if (ret == 1) { /* observation data */ if (iobsn;i++) + for (i = 0;in;i++) { - if (svr->rtk.opt.exsats[obs->data[i].sat-1]==1|| + if (svr->rtk.opt.exsats[obs->data[i].sat-1] == 1 || !(satsys(obs->data[i].sat,NULL)&svr->rtk.opt.navsys)) continue; - svr->obs[index][iobs].data[n]=obs->data[i]; - svr->obs[index][iobs].data[n++].rcv=index+1; + svr->obs[index][iobs].data[n] = obs->data[i]; + svr->obs[index][iobs].data[n++].rcv = index+1; } - svr->obs[index][iobs].n=n; + svr->obs[index][iobs].n = n; sortobs(&svr->obs[index][iobs]); } svr->nmsg[index][0]++; } - else if (ret==2) + else if (ret == 2) { /* ephemeris */ - if (satsys(sat,&prn)!=SYS_GLO) + if (satsys(sat,&prn) != SYS_GLO) { - if (!svr->navsel||svr->navsel==index+1) + if (!svr->navsel || svr->navsel == index+1) { - eph1=nav->eph+sat-1; - eph2=svr->nav.eph+sat-1; - eph3=svr->nav.eph+sat-1+MAXSAT; - if (eph2->ttr.time==0|| - (eph1->iode!=eph3->iode&&eph1->iode!=eph2->iode)|| - (timediff(eph1->toe,eph3->toe)!=0.0&& - timediff(eph1->toe,eph2->toe)!=0.0)) + eph1 = nav->eph+sat-1; + eph2 = svr->nav.eph+sat-1; + eph3 = svr->nav.eph+sat-1+MAXSAT; + if (eph2->ttr.time == 0 || + (eph1->iode != eph3->iode && eph1->iode != eph2->iode) || + (timediff(eph1->toe,eph3->toe) != 0.0 && + timediff(eph1->toe,eph2->toe) != 0.0)) { - *eph3=*eph2; - *eph2=*eph1; + *eph3 = *eph2; + *eph2 = *eph1; updatenav(&svr->nav); } } @@ -160,16 +160,16 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat, } else { - if (!svr->navsel||svr->navsel==index+1) + if (!svr->navsel || svr->navsel == index+1) { - geph1=nav->geph+prn-1; - geph2=svr->nav.geph+prn-1; - geph3=svr->nav.geph+prn-1+MAXPRNGLO; - if (geph2->tof.time==0|| - (geph1->iode!=geph3->iode&&geph1->iode!=geph2->iode)) + geph1 = nav->geph+prn-1; + geph2 = svr->nav.geph+prn-1; + geph3 = svr->nav.geph+prn-1+MAXPRNGLO; + if (geph2->tof.time == 0 || + (geph1->iode != geph3->iode && geph1->iode != geph2->iode)) { - *geph3=*geph2; - *geph2=*geph1; + *geph3 = *geph2; + *geph2 = *geph1; updatenav(&svr->nav); updatefcn(svr); } @@ -177,107 +177,108 @@ void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat, svr->nmsg[index][6]++; } } - else if (ret==3) + else if (ret == 3) { /* sbas message */ - if (sbsmsg&&(sbssat==sbsmsg->prn||sbssat==0)) { + if (sbsmsg && (sbssat == sbsmsg->prn || sbssat == 0)) + { if (svr->nsbssbsmsg[svr->nsbs++]=*sbsmsg; + svr->sbsmsg[svr->nsbs++] = *sbsmsg; } else { - for (i=0;isbsmsg[i]=svr->sbsmsg[i+1]; - svr->sbsmsg[i]=*sbsmsg; + for (i = 0;isbsmsg[i] = svr->sbsmsg[i+1]; + svr->sbsmsg[i] = *sbsmsg; } sbsupdatecorr(sbsmsg,&svr->nav); } svr->nmsg[index][3]++; } - else if (ret==9) + else if (ret == 9) { /* ion/utc parameters */ - if (svr->navsel==index||svr->navsel>=3) + if (svr->navsel == index || svr->navsel >= 3) { - for (i=0;i<8;i++) svr->nav.ion_gps[i]=nav->ion_gps[i]; - for (i=0;i<4;i++) svr->nav.utc_gps[i]=nav->utc_gps[i]; - for (i=0;i<4;i++) svr->nav.ion_gal[i]=nav->ion_gal[i]; - for (i=0;i<4;i++) svr->nav.utc_gal[i]=nav->utc_gal[i]; - for (i=0;i<8;i++) svr->nav.ion_qzs[i]=nav->ion_qzs[i]; - for (i=0;i<4;i++) svr->nav.utc_qzs[i]=nav->utc_qzs[i]; - svr->nav.leaps=nav->leaps; + for (i = 0;i<8;i++) svr->nav.ion_gps[i] = nav->ion_gps[i]; + for (i = 0;i<4;i++) svr->nav.utc_gps[i] = nav->utc_gps[i]; + for (i = 0;i<4;i++) svr->nav.ion_gal[i] = nav->ion_gal[i]; + for (i = 0;i<4;i++) svr->nav.utc_gal[i] = nav->utc_gal[i]; + for (i = 0;i<8;i++) svr->nav.ion_qzs[i] = nav->ion_qzs[i]; + for (i = 0;i<4;i++) svr->nav.utc_qzs[i] = nav->utc_qzs[i]; + svr->nav.leaps = nav->leaps; } svr->nmsg[index][2]++; } - else if (ret==5) + else if (ret == 5) { /* antenna postion parameters */ - if (svr->rtk.opt.refpos==4&&index==1) + if (svr->rtk.opt.refpos == 4 && index == 1) { - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { - svr->rtk.rb[i]=svr->rtcm[1].sta.pos[i]; + svr->rtk.rb[i] = svr->rtcm[1].sta.pos[i]; } /* antenna delta */ ecef2pos(svr->rtk.rb,pos); if (svr->rtcm[1].sta.deltype) { /* xyz */ - del[2]=svr->rtcm[1].sta.hgt; + del[2] = svr->rtcm[1].sta.hgt; enu2ecef(pos,del,dr); - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { - svr->rtk.rb[i]+=svr->rtcm[1].sta.del[i]+dr[i]; + svr->rtk.rb[i] += svr->rtcm[1].sta.del[i]+dr[i]; } } else { /* enu */ enu2ecef(pos,svr->rtcm[1].sta.del,dr); - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { - svr->rtk.rb[i]+=dr[i]; + svr->rtk.rb[i] += dr[i]; } } } svr->nmsg[index][4]++; } - else if (ret==7) + else if (ret == 7) { /* dgps correction */ svr->nmsg[index][5]++; } - else if (ret==10) + else if (ret == 10) { /* ssr message */ - for (i=0;irtcm[index].ssr[i].update) continue; - svr->rtcm[index].ssr[i].update=0; + svr->rtcm[index].ssr[i].update = 0; - iode=svr->rtcm[index].ssr[i].iode; - sys=satsys(i+1,&prn); + iode = svr->rtcm[index].ssr[i].iode; + sys = satsys(i+1,&prn); /* check corresponding ephemeris exists */ - if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS) + if (sys == SYS_GPS || sys == SYS_GAL || sys == SYS_QZS) { - if (svr->nav.eph[i ].iode!=iode&& - svr->nav.eph[i+MAXSAT].iode!=iode) + if (svr->nav.eph[i ].iode != iode && + svr->nav.eph[i+MAXSAT].iode != iode) { continue; } } - else if (sys==SYS_GLO) + else if (sys == SYS_GLO) { - if (svr->nav.geph[prn-1 ].iode!=iode&& - svr->nav.geph[prn-1+MAXPRNGLO].iode!=iode) + if (svr->nav.geph[prn-1 ].iode != iode && + svr->nav.geph[prn-1+MAXPRNGLO].iode != iode) { continue; } } - svr->nav.ssr[i]=svr->rtcm[index].ssr[i]; + svr->nav.ssr[i] = svr->rtcm[index].ssr[i]; } svr->nmsg[index][7]++; } - else if (ret==31) + else if (ret == 31) { /* lex message */ // lexupdatecorr(&svr->raw[index].lexmsg,&svr->nav,&tof); svr->nmsg[index][8]++; } - else if (ret==-1) + else if (ret == -1) { /* error */ svr->nmsg[index][9]++; } @@ -289,38 +290,38 @@ int decoderaw(rtksvr_t *svr, int index) { obs_t *obs; nav_t *nav; - sbsmsg_t *sbsmsg=NULL; - int i,ret,sat,fobs=0; + sbsmsg_t *sbsmsg = NULL; + int i,ret,sat,fobs = 0; tracet(4,"decoderaw: index=%d\n",index); rtksvrlock(svr); - for (i=0;inb[index];i++) + for (i = 0;inb[index];i++) { /* input rtcm/receiver raw data from stream */ - if (svr->format[index]==STRFMT_RTCM2) + if (svr->format[index] == STRFMT_RTCM2) { - ret=input_rtcm2(svr->rtcm+index,svr->buff[index][i]); - obs=&svr->rtcm[index].obs; - nav=&svr->rtcm[index].nav; - sat=svr->rtcm[index].ephsat; + ret = input_rtcm2(svr->rtcm+index,svr->buff[index][i]); + obs = &svr->rtcm[index].obs; + nav = &svr->rtcm[index].nav; + sat = svr->rtcm[index].ephsat; } - else if (svr->format[index]==STRFMT_RTCM3) + else if (svr->format[index] == STRFMT_RTCM3) { - ret=input_rtcm3(svr->rtcm+index,svr->buff[index][i]); - obs=&svr->rtcm[index].obs; - nav=&svr->rtcm[index].nav; - sat=svr->rtcm[index].ephsat; + ret = input_rtcm3(svr->rtcm+index,svr->buff[index][i]); + obs = &svr->rtcm[index].obs; + nav = &svr->rtcm[index].nav; + sat = svr->rtcm[index].ephsat; } else { // Disabled !! - //ret=input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]); - obs=&svr->raw[index].obs; - nav=&svr->raw[index].nav; - sat=svr->raw[index].ephsat; - sbsmsg=&svr->raw[index].sbsmsg; + //ret = input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]); + obs = &svr->raw[index].obs; + nav = &svr->raw[index].nav; + sat = svr->raw[index].ephsat; + sbsmsg = &svr->raw[index].sbsmsg; } #if 0 /* record for receiving tick */ if (ret==1) { @@ -332,11 +333,11 @@ int decoderaw(rtksvr_t *svr, int index) if (ret>0) updatesvr(svr,ret,obs,nav,sat,sbsmsg,index,fobs); /* observation data received */ - if (ret==1) { + if (ret == 1) { if (fobsprcout++; } } - svr->nb[index]=0; + svr->nb[index] = 0; rtksvrunlock(svr); @@ -347,7 +348,7 @@ int decoderaw(rtksvr_t *svr, int index) /* decode download file ------------------------------------------------------*/ void decodefile(rtksvr_t *svr, int index) { - nav_t nav={0}; + nav_t nav = {0}; char file[1024]; int nb; @@ -356,21 +357,23 @@ void decodefile(rtksvr_t *svr, int index) rtksvrlock(svr); /* check file path completed */ - if ((nb=svr->nb[index])<=2|| - svr->buff[index][nb-2]!='\r'||svr->buff[index][nb-1]!='\n') { + if ((nb = svr->nb[index]) <= 2 || + svr->buff[index][nb-2] != '\r' || svr->buff[index][nb-1] != '\n') + { rtksvrunlock(svr); return; } - strncpy(file,(char *)svr->buff[index],nb-2); file[nb-2]='\0'; - svr->nb[index]=0; + strncpy(file,(char *)svr->buff[index],nb-2); file[nb-2] = '\0'; + svr->nb[index] = 0; rtksvrunlock(svr); - if (svr->format[index]==STRFMT_SP3) { /* precise ephemeris */ + if (svr->format[index] == STRFMT_SP3) { /* precise ephemeris */ /* read sp3 precise ephemeris */ readsp3(file,&nav,0); - if (nav.ne<=0) { + if (nav.ne <= 0) + { tracet(1,"sp3 file read error: %s\n",file); return; } @@ -378,14 +381,14 @@ void decodefile(rtksvr_t *svr, int index) rtksvrlock(svr); if (svr->nav.peph) free(svr->nav.peph); - svr->nav.ne=svr->nav.nemax=nav.ne; - svr->nav.peph=nav.peph; - svr->ftime[index]=utc2gpst(timeget()); + svr->nav.ne = svr->nav.nemax = nav.ne; + svr->nav.peph = nav.peph; + svr->ftime[index] = utc2gpst(timeget()); strcpy(svr->files[index],file); rtksvrunlock(svr); } - else if (svr->format[index]==STRFMT_RNXCLK) { /* precise clock */ + else if (svr->format[index] == STRFMT_RNXCLK) { /* precise clock */ /* read rinex clock */ // Disabled!! if ( 1 /*readrnxc(file,&nav)<=0 */) { @@ -396,9 +399,9 @@ void decodefile(rtksvr_t *svr, int index) rtksvrlock(svr); if (svr->nav.pclk) free(svr->nav.pclk); - svr->nav.nc=svr->nav.ncmax=nav.nc; - svr->nav.pclk=nav.pclk; - svr->ftime[index]=utc2gpst(timeget()); + svr->nav.nc = svr->nav.ncmax = nav.nc; + svr->nav.pclk = nav.pclk; + svr->ftime[index] = utc2gpst(timeget()); strcpy(svr->files[index],file); rtksvrunlock(svr); @@ -409,47 +412,47 @@ void decodefile(rtksvr_t *svr, int index) /* rtk server thread ---------------------------------------------------------*/ void *rtksvrthread(void *arg) { - rtksvr_t *svr=(rtksvr_t *)arg; + rtksvr_t *svr = (rtksvr_t *)arg; obs_t obs; obsd_t data[MAXOBS*2]; double tt; unsigned int tick,ticknmea; unsigned char *p,*q; - int i,j,n,fobs[3]={0},cycle,cputime; + int i,j,n,fobs[3] = {0},cycle,cputime; tracet(3,"rtksvrthread:\n"); - svr->state=1; obs.data=data; - svr->tick=tickget(); - ticknmea=svr->tick-1000; + svr->state = 1; obs.data = data; + svr->tick = tickget(); + ticknmea = svr->tick-1000; - for (cycle=0;svr->state;cycle++) + for (cycle = 0;svr->state;cycle++) { - tick=tickget(); + tick = tickget(); - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { - p=svr->buff[i]+svr->nb[i]; q=svr->buff[i]+svr->buffsize; + p = svr->buff[i]+svr->nb[i]; q = svr->buff[i]+svr->buffsize; /* read receiver raw/rtcm data from input stream */ - if ((n=strread(svr->stream+i,p,q-p))<=0) + if ((n = strread(svr->stream+i,p,q-p)) <= 0) { continue; } /* write receiver raw/rtcm data to log stream */ strwrite(svr->stream+i+5,p,n); - svr->nb[i]+=n; + svr->nb[i] += n; /* save peek buffer */ rtksvrlock(svr); - n=nbuffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i]; + n = nbuffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i]; memcpy(svr->pbuf[i]+svr->npb[i],p,n); - svr->npb[i]+=n; + svr->npb[i] += n; rtksvrunlock(svr); } - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { - if (svr->format[i]==STRFMT_SP3||svr->format[i]==STRFMT_RNXCLK) + if (svr->format[i] == STRFMT_SP3 || svr->format[i] == STRFMT_RNXCLK) { /* decode download file */ decodefile(svr,i); @@ -457,82 +460,82 @@ void *rtksvrthread(void *arg) else { /* decode receiver raw/rtcm data */ - fobs[i]=decoderaw(svr,i); + fobs[i] = decoderaw(svr,i); } } - for (i=0;iobs[0][i].n&&obs.nobs[0][i].n && obs.nobs[0][i].data[j]; + obs.data[obs.n++] = svr->obs[0][i].data[j]; } - for (j=0;jobs[1][0].n&&obs.nobs[1][0].n && obs.nobs[1][0].data[j]; + obs.data[obs.n++] = svr->obs[1][0].data[j]; } /* rtk positioning */ rtksvrlock(svr); rtkpos(&svr->rtk,obs.data,obs.n,&svr->nav); rtksvrunlock(svr); - if (svr->rtk.sol.stat!=SOLQ_NONE) + if (svr->rtk.sol.stat != SOLQ_NONE) { /* adjust current time */ - tt=(int)(tickget()-tick)/1000.0+DTTOL; + tt = (int)(tickget()-tick)/1000.0+DTTOL; timeset(gpst2utc(timeadd(svr->rtk.sol.time,tt))); /* write solution */ writesol(svr,i); } /* if cpu overload, inclement obs outage counter and break */ - if ((int)(tickget()-tick)>=svr->cycle) + if ((int)(tickget()-tick) >= svr->cycle) { - svr->prcout+=fobs[0]-i-1; + svr->prcout += fobs[0]-i-1; #if 0 /* omitted v.2.4.1 */ break; #endif } } /* send null solution if no solution (1hz) */ - if (svr->rtk.sol.stat==SOLQ_NONE&&cycle%(1000/svr->cycle)==0) + if (svr->rtk.sol.stat == SOLQ_NONE && cycle%(1000/svr->cycle) == 0) { writesol(svr,0); } /* send nmea request to base/nrtk input stream */ - if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle) + if (svr->nmeacycle>0 && (int)(tick-ticknmea) >= svr->nmeacycle) { - if (svr->stream[1].state==1) + if (svr->stream[1].state == 1) { - if (svr->nmeareq==1) + if (svr->nmeareq == 1) { strsendnmea(svr->stream+1,svr->nmeapos); } - else if (svr->nmeareq==2&&norm_rtk(svr->rtk.sol.rr,3)>0.0) + else if (svr->nmeareq == 2 && norm_rtk(svr->rtk.sol.rr,3)>0.0) { strsendnmea(svr->stream+1,svr->rtk.sol.rr); } } - ticknmea=tick; + ticknmea = tick; } - if ((cputime=(int)(tickget()-tick))>0) svr->cputime=cputime; + if ((cputime = (int)(tickget()-tick))>0) svr->cputime = cputime; /* sleep until next cycle */ sleepms(svr->cycle-cputime); } - for (i=0;istream+i); - for (i=0;i<3;i++) + for (i = 0;istream+i); + for (i = 0;i<3;i++) { - svr->nb[i]=svr->npb[i]=0; - free(svr->buff[i]); svr->buff[i]=NULL; - free(svr->pbuf[i]); svr->pbuf[i]=NULL; + svr->nb[i] = svr->npb[i] = 0; + free(svr->buff[i]); svr->buff[i] = NULL; + free(svr->pbuf[i]); svr->pbuf[i] = NULL; //free_raw (svr->raw +i); free_rtcm(svr->rtcm+i); } - for (i=0;i<2;i++) + for (i = 0;i<2;i++) { - svr->nsb[i]=0; - free(svr->sbuf[i]); svr->sbuf[i]=NULL; + svr->nsb[i] = 0; + free(svr->sbuf[i]); svr->sbuf[i] = NULL; } return 0; } @@ -545,65 +548,65 @@ void *rtksvrthread(void *arg) *-----------------------------------------------------------------------------*/ int rtksvrinit(rtksvr_t *svr) { - gtime_t time0={0}; - sol_t sol0 ={{0}}; - eph_t eph0 ={0,-1,-1}; - geph_t geph0={0,-1}; - seph_t seph0={0}; + gtime_t time0 = {0}; + sol_t sol0 = {{0}}; + eph_t eph0 = {0,-1,-1}; + geph_t geph0 = {0,-1}; + seph_t seph0 = {0}; int i,j; tracet(3,"rtksvrinit:\n"); - svr->state=svr->cycle=svr->nmeacycle=svr->nmeareq=0; - for (i=0;i<3;i++) svr->nmeapos[i]=0.0; - svr->buffsize=0; - for (i=0;i<3;i++) svr->format[i]=0; - for (i=0;i<2;i++) svr->solopt[i]=solopt_default; - svr->navsel=svr->nsbs=svr->nsol=0; + svr->state = svr->cycle = svr->nmeacycle = svr->nmeareq = 0; + for (i = 0;i<3;i++) svr->nmeapos[i] = 0.0; + svr->buffsize = 0; + for (i = 0;i<3;i++) svr->format[i] = 0; + for (i = 0;i<2;i++) svr->solopt[i] = solopt_default; + svr->navsel = svr->nsbs = svr->nsol = 0; rtkinit(&svr->rtk,&prcopt_default); - for (i=0;i<3;i++) svr->nb[i]=0; - for (i=0;i<2;i++) svr->nsb[i]=0; - for (i=0;i<3;i++) svr->npb[i]=0; - for (i=0;i<3;i++) svr->buff[i]=NULL; - for (i=0;i<2;i++) svr->sbuf[i]=NULL; - for (i=0;i<3;i++) svr->pbuf[i]=NULL; - for (i=0;isolbuf[i]=sol0; - for (i=0;i<3;i++) for (j=0;j<10;j++) svr->nmsg[i][j]=0; - for (i=0;i<3;i++) svr->ftime[i]=time0; - for (i=0;i<3;i++) svr->files[i][0]='\0'; - svr->moni=NULL; - svr->tick=0; - svr->thread=0; - svr->cputime=svr->prcout=0; + for (i = 0;i<3;i++) svr->nb[i] = 0; + for (i = 0;i<2;i++) svr->nsb[i] = 0; + for (i = 0;i<3;i++) svr->npb[i] = 0; + for (i = 0;i<3;i++) svr->buff[i] = NULL; + for (i = 0;i<2;i++) svr->sbuf[i] = NULL; + for (i = 0;i<3;i++) svr->pbuf[i] = NULL; + for (i = 0;isolbuf[i] = sol0; + for (i = 0;i<3;i++) for (j = 0;j<10;j++) svr->nmsg[i][j] = 0; + for (i = 0;i<3;i++) svr->ftime[i] = time0; + for (i = 0;i<3;i++) svr->files[i][0] = '\0'; + svr->moni = NULL; + svr->tick = 0; + svr->thread = 0; + svr->cputime = svr->prcout = 0; - if (!(svr->nav.eph =(eph_t *)malloc(sizeof(eph_t )*MAXSAT *2))|| - !(svr->nav.geph=(geph_t *)malloc(sizeof(geph_t)*NSATGLO*2))|| - !(svr->nav.seph=(seph_t *)malloc(sizeof(seph_t)*NSATSBS*2))) + if (!(svr->nav.eph = (eph_t *)malloc(sizeof(eph_t )*MAXSAT *2)) || + !(svr->nav.geph = (geph_t *)malloc(sizeof(geph_t)*NSATGLO*2)) || + !(svr->nav.seph = (seph_t *)malloc(sizeof(seph_t)*NSATSBS*2))) { tracet(1,"rtksvrinit: malloc error\n"); return 0; } - for (i=0;inav.eph [i]=eph0; - for (i=0;inav.geph[i]=geph0; - for (i=0;inav.seph[i]=seph0; - svr->nav.n =MAXSAT *2; - svr->nav.ng=NSATGLO*2; - svr->nav.ns=NSATSBS*2; + for (i = 0;inav.eph [i] = eph0; + for (i = 0;inav.geph[i] = geph0; + for (i = 0;inav.seph[i] = seph0; + svr->nav.n = MAXSAT *2; + svr->nav.ng = NSATGLO*2; + svr->nav.ns = NSATSBS*2; - for (i=0;i<3;i++) for (j=0;jobs[i][j].data=(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))) + if (!(svr->obs[i][j].data = (obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))) { tracet(1,"rtksvrinit: malloc error\n"); return 0; } } - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { memset(svr->raw +i,0,sizeof(raw_t )); memset(svr->rtcm+i,0,sizeof(rtcm_t)); } - for (i=0;istream+i); + for (i = 0;istream+i); initlock(&svr->lock); @@ -623,7 +626,7 @@ void *rtksvrthread(void *arg) free(svr->nav.eph ); free(svr->nav.geph); free(svr->nav.seph); - for (i=0;i<3;i++) for (j=0;jobs[i][j].data); } @@ -683,7 +686,7 @@ void *rtksvrthread(void *arg) const double *nmeapos, prcopt_t *prcopt, solopt_t *solopt, stream_t *moni) { - gtime_t time,time0={0}; + gtime_t time,time0 = {0}; int i,j,rw; tracet(3,"rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n", @@ -692,30 +695,30 @@ void *rtksvrthread(void *arg) if (svr->state) return 0; strinitcom(); - svr->cycle=cycle>1?cycle:1; - svr->nmeacycle=nmeacycle>1000?nmeacycle:1000; - svr->nmeareq=nmeareq; - for (i=0;i<3;i++) svr->nmeapos[i]=nmeapos[i]; - svr->buffsize=buffsize>4096?buffsize:4096; - for (i=0;i<3;i++) svr->format[i]=formats[i]; - svr->navsel=navsel; - svr->nsbs=0; - svr->nsol=0; - svr->prcout=0; + svr->cycle = cycle>1?cycle:1; + svr->nmeacycle = nmeacycle>1000?nmeacycle:1000; + svr->nmeareq = nmeareq; + for (i = 0;i<3;i++) svr->nmeapos[i] = nmeapos[i]; + svr->buffsize = buffsize>4096?buffsize:4096; + for (i = 0;i<3;i++) svr->format[i] = formats[i]; + svr->navsel = navsel; + svr->nsbs = 0; + svr->nsol = 0; + svr->prcout = 0; rtkfree(&svr->rtk); rtkinit(&svr->rtk,prcopt); - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { /* input/log streams */ - svr->nb[i]=svr->npb[i]=0; - if (!(svr->buff[i]=(unsigned char *)malloc(buffsize))|| - !(svr->pbuf[i]=(unsigned char *)malloc(buffsize))) + svr->nb[i] = svr->npb[i] = 0; + if (!(svr->buff[i] = (unsigned char *)malloc(buffsize)) || + !(svr->pbuf[i] = (unsigned char *)malloc(buffsize))) { tracet(1,"rtksvrstart: malloc error\n"); return 0; } - for (j=0;j<10;j++) svr->nmsg[i][j]=0; - for (j=0;jobs[i][j].n=0; + for (j = 0;j<10;j++) svr->nmsg[i][j] = 0; + for (j = 0;jobs[i][j].n = 0; /* initialize receiver raw and rtcm control */ //init_raw (svr->raw +i); @@ -726,51 +729,51 @@ void *rtksvrthread(void *arg) strcpy(svr->rtcm[i].opt,rcvopts[i]); /* connect dgps corrections */ - svr->rtcm[i].dgps=svr->nav.dgps; + svr->rtcm[i].dgps = svr->nav.dgps; } - for (i=0;i<2;i++) + for (i = 0;i<2;i++) { /* output peek buffer */ - if (!(svr->sbuf[i]=(unsigned char *)malloc(buffsize))) + if (!(svr->sbuf[i] = (unsigned char *)malloc(buffsize))) { tracet(1,"rtksvrstart: malloc error\n"); return 0; } } /* set solution options */ - for (i=0;i<2;i++) + for (i = 0;i<2;i++) { - svr->solopt[i]=solopt[i]; + svr->solopt[i] = solopt[i]; } /* set base station position */ - for (i=0;i<6;i++) + for (i = 0;i<6;i++) { - svr->rtk.rb[i]=i<3?prcopt->rb[i]:0.0; + svr->rtk.rb[i] = i<3?prcopt->rb[i]:0.0; } /* update navigation data */ - for (i=0;inav.eph [i].ttr=time0; - for (i=0;inav.geph[i].tof=time0; - for (i=0;inav.seph[i].tof=time0; + for (i = 0;inav.eph [i].ttr = time0; + for (i = 0;inav.geph[i].tof = time0; + for (i = 0;inav.seph[i].tof = time0; updatenav(&svr->nav); /* set monitor stream */ - svr->moni=moni; + svr->moni = moni; /* open input streams */ - for (i=0;i<8;i++) + for (i = 0;i<8;i++) { - rw=i<3?STR_MODE_R:STR_MODE_W; - if (strs[i]!=STR_FILE) rw|=STR_MODE_W; + rw = i<3?STR_MODE_R:STR_MODE_W; + if (strs[i] != STR_FILE) rw |= STR_MODE_W; if (!stropen(svr->stream+i,strs[i],rw,paths[i])) { - for (i--;i>=0;i--) strclose(svr->stream+i); + for (i--;i >= 0;i--) strclose(svr->stream+i); return 0; } /* set initial time for rtcm and raw */ if (i<3) { - time=utc2gpst(timeget()); - svr->raw [i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time; - svr->rtcm[i].time=strs[i]==STR_FILE?strgettime(svr->stream+i):time; + time = utc2gpst(timeget()); + svr->raw [i].time = strs[i] == STR_FILE?strgettime(svr->stream+i):time; + svr->rtcm[i].time = strs[i] == STR_FILE?strgettime(svr->stream+i):time; } } /* sync input streams */ @@ -778,24 +781,24 @@ void *rtksvrthread(void *arg) strsync(svr->stream,svr->stream+2); /* write start commands to input streams */ - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]); } /* write solution header to solution streams */ - for (i=3;i<5;i++) + for (i = 3;i<5;i++) { writesolhead(svr->stream+i,svr->solopt+i-3); } /* create rtk server thread */ #ifdef WIN32 - if (!(svr->thread=CreateThread(NULL,0,rtksvrthread,svr,0,NULL))) + if (!(svr->thread = CreateThread(NULL,0,rtksvrthread,svr,0,NULL))) { #else if (pthread_create(&svr->thread,NULL,rtksvrthread,svr)) { #endif - for (i=0;istream+i); + for (i = 0;istream+i); return 0; } return 1; @@ -819,14 +822,14 @@ void *rtksvrthread(void *arg) /* write stop commands to input streams */ rtksvrlock(svr); - for (i=0;i<3;i++) + for (i = 0;i<3;i++) { if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]); } rtksvrunlock(svr); /* stop rtk server */ - svr->state=0; + svr->state = 0; /* free rtk server thread */ #ifdef WIN32 @@ -854,7 +857,7 @@ void *rtksvrthread(void *arg) { tracet(3,"rtksvropenstr: index=%d str=%d path=%s\n",index,str,path); - if (index<3||index>7||!svr->state) return 0; + if (index<3 || index>7 || !svr->state) return 0; rtksvrlock(svr); @@ -869,9 +872,9 @@ void *rtksvrthread(void *arg) rtksvrunlock(svr); return 0; } - if (index<=4) + if (index <= 4) { - svr->solopt[index-3]=*solopt; + svr->solopt[index-3] = *solopt; /* write solution header to solution stream */ writesolhead(svr->stream+index,svr->solopt+index-3); @@ -893,7 +896,7 @@ void *rtksvrthread(void *arg) { tracet(3,"rtksvrclosestr: index=%d\n",index); - if (index<3||index>7||!svr->state) return; + if (index<3 || index>7 || !svr->state) return; rtksvrlock(svr); @@ -925,27 +928,27 @@ void *rtksvrthread(void *arg) if (!svr->state) return 0; rtksvrlock(svr); - ns=svr->obs[rcv][0].n; + ns = svr->obs[rcv][0].n; if (ns>0) { - *time=svr->obs[rcv][0].data[0].time; + *time = svr->obs[rcv][0].data[0].time; } - for (i=0;iobs[rcv][0].data[i].sat; - az [i]=svr->rtk.ssat[sat[i]-1].azel[0]; - el [i]=svr->rtk.ssat[sat[i]-1].azel[1]; - for (j=0;jobs[rcv][0].data[i].sat; + az [i] = svr->rtk.ssat[sat[i]-1].azel[0]; + el [i] = svr->rtk.ssat[sat[i]-1].azel[1]; + for (j = 0;jobs[rcv][0].data[i].SNR[j]*0.25); + snr[i][j] = (int)(svr->obs[rcv][0].data[i].SNR[j]*0.25); } - if (svr->rtk.sol.stat==SOLQ_NONE||svr->rtk.sol.stat==SOLQ_SINGLE) + if (svr->rtk.sol.stat == SOLQ_NONE || svr->rtk.sol.stat == SOLQ_SINGLE) { - vsat[i]=svr->rtk.ssat[sat[i]-1].vs; + vsat[i] = svr->rtk.ssat[sat[i]-1].vs; } else { - vsat[i]=svr->rtk.ssat[sat[i]-1].vsat[0]; + vsat[i] = svr->rtk.ssat[sat[i]-1].vsat[0]; } } rtksvrunlock(svr); @@ -963,15 +966,15 @@ void *rtksvrthread(void *arg) void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg) { int i; - char s[MAXSTRMSG],*p=msg; + char s[MAXSTRMSG],*p = msg; tracet(4,"rtksvrsstat:\n"); rtksvrlock(svr); - for (i=0;istream+i,s); - if (*s) p+=sprintf(p,"(%d) %s ",i+1,s); + sstat[i] = strstat(svr->stream+i,s); + if (*s) p += sprintf(p,"(%d) %s ",i+1,s); } rtksvrunlock(svr); } diff --git a/src/algorithms/libs/rtklib/rtklib_solution.cc b/src/algorithms/libs/rtklib/rtklib_solution.cc index c26f7a5b2..af701cd84 100644 --- a/src/algorithms/libs/rtklib/rtklib_solution.cc +++ b/src/algorithms/libs/rtklib/rtklib_solution.cc @@ -53,6 +53,8 @@ #include #include "rtklib_solution.h" +#include "rtklib_rtkcmn.h" +#include "rtklib_rtksvr.h" /* constants and macros ------------------------------------------------------*/ @@ -60,16 +62,15 @@ #define SQR_SOL(x) ((x)<0.0?-(x)*(x):(x)*(x)) #define SQRT_SOL(x) ((x)<0.0?0.0:sqrt(x)) -#define MAXFIELD 64 /* max number of fields in a record */ -#define MAXNMEA 256 /* max length of nmea sentence */ +const int MAXFIELD = 64; /* max number of fields in a record */ +const int MAXNMEA = 256; /* max length of nmea sentence */ -#define KNOT2M 0.514444444 /* m/knot */ +const double KNOT2M = 0.514444444; /* m/knot */ -static const int solq_nmea[]={ /* nmea quality flags to rtklib sol quality */ +static const int solq_nmea[] = { /* nmea quality flags to rtklib sol quality */ /* nmea 0183 v.2.3 quality flags: */ /* 0=invalid, 1=gps fix (sps), 2=dgps fix, 3=pps fix, 4=rtk, 5=float rtk */ /* 6=estimated (dead reckoning), 7=manual input, 8=simulation */ - SOLQ_NONE ,SOLQ_SINGLE, SOLQ_DGPS, SOLQ_PPP , SOLQ_FIX, SOLQ_FLOAT,SOLQ_DR , SOLQ_NONE, SOLQ_NONE, SOLQ_NONE }; @@ -87,13 +88,13 @@ const char *opt2sep(const solopt_t *opt) /* separate fields -----------------------------------------------------------*/ int tonum(char *buff, const char *sep, double *v) { - int n,len=(int)strlen(sep); + int n,len = (int)strlen(sep); char *p,*q; - for (p=buff,n=0;nqr[0]; /* xx or ee */ - P[4] =sol->qr[1]; /* yy or nn */ - P[8] =sol->qr[2]; /* zz or uu */ - P[1]=P[3]=sol->qr[3]; /* xy or en */ - P[5]=P[7]=sol->qr[4]; /* yz or nu */ - P[2]=P[6]=sol->qr[5]; /* zx or ue */ + P[0] = sol->qr[0]; /* xx or ee */ + P[4] = sol->qr[1]; /* yy or nn */ + P[8] = sol->qr[2]; /* zz or uu */ + P[1] = P[3] = sol->qr[3]; /* xy or en */ + P[5] = P[7] = sol->qr[4]; /* yz or nu */ + P[2] = P[6] = sol->qr[5]; /* zx or ue */ } /* covariance to solution ----------------------------------------------------*/ void covtosol(const double *P, sol_t *sol) { - sol->qr[0]=(float)P[0]; /* xx or ee */ - sol->qr[1]=(float)P[4]; /* yy or nn */ - sol->qr[2]=(float)P[8]; /* zz or uu */ - sol->qr[3]=(float)P[1]; /* xy or en */ - sol->qr[4]=(float)P[5]; /* yz or nu */ - sol->qr[5]=(float)P[2]; /* zx or ue */ + sol->qr[0] = (float)P[0]; /* xx or ee */ + sol->qr[1] = (float)P[4]; /* yy or nn */ + sol->qr[2] = (float)P[8]; /* zz or uu */ + sol->qr[3] = (float)P[1]; /* xy or en */ + sol->qr[4] = (float)P[5]; /* yz or nu */ + sol->qr[5] = (float)P[2]; /* zx or ue */ } /* decode nmea gprmc: recommended minumum data for gps -----------------------*/ int decode_nmearmc(char **val, int n, sol_t *sol) { - double tod=0.0,lat=0.0,lon=0.0,vel=0.0,dir=0.0,date=0.0,ang=0.0,ep[6]; - double pos[3]={0}; - char act=' ',ns='N',ew='E',mew='E',mode='A'; + double tod = 0.0,lat = 0.0,lon = 0.0,vel = 0.0,dir = 0.0,date = 0.0,ang = 0.0,ep[6]; + double pos[3] = {0}; + char act = ' ',ns = 'N',ew = 'E',mew = 'E',mode = 'A'; int i; trace(4,"decode_nmearmc: n=%d\n",n); - for (i=0;inmea 2) */ + case 0: tod = atof(val[i]); break; /* time in utc (hhmmss) */ + case 1: act = *val[i]; break; /* A=active,V=void */ + case 2: lat = atof(val[i]); break; /* latitude (ddmm.mmm) */ + case 3: ns = *val[i]; break; /* N=north,S=south */ + case 4: lon = atof(val[i]); break; /* longitude (dddmm.mmm) */ + case 5: ew = *val[i]; break; /* E=east,W=west */ + case 6: vel = atof(val[i]); break; /* speed (knots) */ + case 7: dir = atof(val[i]); break; /* track angle (deg) */ + case 8: date = atof(val[i]); break; /* date (ddmmyy) */ + case 9: ang = atof(val[i]); break; /* magnetic variation */ + case 10: mew = *val[i]; break; /* E=east,W=west */ + case 11: mode = *val[i]; break; /* mode indicator (>nmea 2) */ /* A=autonomous,D=differential */ /* E=estimated,N=not valid,S=simulator */ } } - if ((act!='A'&&act!='V')||(ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) + if ((act != 'A' && act != 'V') || (ns != 'N' && ns != 'S') || (ew != 'E' && ew != 'W')) { trace(2,"invalid nmea gprmc format\n"); return 0; } - pos[0]=(ns=='S'?-1.0:1.0)*dmm2deg(lat)*D2R; - pos[1]=(ew=='W'?-1.0:1.0)*dmm2deg(lon)*D2R; + pos[0] = (ns == 'S'?-1.0:1.0)*dmm2deg(lat)*D2R; + pos[1] = (ew == 'W'?-1.0:1.0)*dmm2deg(lon)*D2R; septime(date,ep+2,ep+1,ep); septime(tod,ep+3,ep+4,ep+5); - ep[0]+=ep[0]<80.0?2000.0:1900.0; - sol->time=utc2gpst(epoch2time(ep)); + ep[0] += ep[0]<80.0?2000.0:1900.0; + sol->time = utc2gpst(epoch2time(ep)); pos2ecef(pos,sol->rr); - sol->stat=mode=='D'?SOLQ_DGPS:SOLQ_SINGLE; - sol->ns=0; + sol->stat = mode == 'D'?SOLQ_DGPS:SOLQ_SINGLE; + sol->ns = 0; - sol->type=0; /* postion type = xyz */ + sol->type = 0; /* postion type = xyz */ trace(5,"decode_nmearmc: %s rr=%.3f %.3f %.3f stat=%d ns=%d vel=%.2f dir=%.0f ang=%.0f mew=%c mode=%c\n", time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns, @@ -207,57 +208,57 @@ int decode_nmearmc(char **val, int n, sol_t *sol) int decode_nmeagga(char **val, int n, sol_t *sol) { gtime_t time; - double tod=0.0,lat=0.0,lon=0.0,hdop=0.0,alt=0.0,msl=0.0,ep[6],tt; - double pos[3]={0}; - char ns='N',ew='E',ua=' ',um=' '; - int i,solq=0,nrcv=0; + double tod = 0.0,lat = 0.0,lon = 0.0,hdop = 0.0,alt = 0.0,msl = 0.0,ep[6],tt; + double pos[3] = {0}; + char ns = 'N',ew = 'E',ua = ' ',um = ' '; + int i,solq = 0,nrcv = 0; trace(4,"decode_nmeagga: n=%d\n",n); - for (i=0;itime.time==0.0) + if (sol->time.time == 0.0) { trace(2,"no date info for nmea gpgga\n"); return 0; } - pos[0]=(ns=='N'?1.0:-1.0)*dmm2deg(lat)*D2R; - pos[1]=(ew=='E'?1.0:-1.0)*dmm2deg(lon)*D2R; - pos[2]=alt+msl; + pos[0] = (ns == 'N'?1.0:-1.0)*dmm2deg(lat)*D2R; + pos[1] = (ew == 'E'?1.0:-1.0)*dmm2deg(lon)*D2R; + pos[2] = alt+msl; time2epoch(sol->time,ep); septime(tod,ep+3,ep+4,ep+5); - time=utc2gpst(epoch2time(ep)); - tt=timediff(time,sol->time); - if (tt<-43200.0) sol->time=timeadd(time, 86400.0); - else if (tt> 43200.0) sol->time=timeadd(time,-86400.0); - else sol->time=time; + time = utc2gpst(epoch2time(ep)); + tt = timediff(time,sol->time); + if (tt<-43200.0) sol->time = timeadd(time, 86400.0); + else if (tt> 43200.0) sol->time = timeadd(time,-86400.0); + else sol->time = time; pos2ecef(pos,sol->rr); - sol->stat=0<=solq&&solq<=8?solq_nmea[solq]:SOLQ_NONE; - sol->ns=nrcv; + sol->stat = 0 <= solq && solq <= 8?solq_nmea[solq]:SOLQ_NONE; + sol->ns = nrcv; - sol->type=0; /* postion type = xyz */ + sol->type = 0; /* postion type = xyz */ trace(5,"decode_nmeagga: %s rr=%.3f %.3f %.3f stat=%d ns=%d hdop=%.1f ua=%c um=%c\n", time_str(sol->time,0),sol->rr[0],sol->rr[1],sol->rr[2],sol->stat,sol->ns, @@ -271,16 +272,16 @@ int decode_nmeagga(char **val, int n, sol_t *sol) int decode_nmea(char *buff, sol_t *sol) { char *p,*q,*val[MAXFIELD]; - int n=0; + int n = 0; trace(4,"decode_nmea: buff=%s\n",buff); /* parse fields */ - for (p=buff;*p&&nsep,"\\t")) strcpy(s,"\t"); else if (*opt->sep) strcpy(s,opt->sep); - len=(int)strlen(s); + len = (int)strlen(s); /* yyyy/mm/dd hh:mm:ss or yyyy mm dd hh:mm:ss */ - if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf",v,v+1,v+2,v+3,v+4,v+5)>=6) + if (sscanf(buff,"%lf/%lf/%lf %lf:%lf:%lf",v,v+1,v+2,v+3,v+4,v+5) >= 6) { if (v[0]<100.0) { - v[0]+=v[0]<80.0?2000.0:1900.0; + v[0] += v[0]<80.0?2000.0:1900.0; } - *time=epoch2time(v); - if (opt->times==TIMES_UTC) + *time = epoch2time(v); + if (opt->times == TIMES_UTC) { - *time=utc2gpst(*time); + *time = utc2gpst(*time); } - else if (opt->times==TIMES_JST) + else if (opt->times == TIMES_JST) { - *time=utc2gpst(timeadd(*time,-9*3600.0)); + *time = utc2gpst(timeadd(*time,-9*3600.0)); } - if (!(p=strchr(buff,':'))||!(p=strchr(p+1,':'))) return NULL; - for (p++;isdigit((int)*p)||*p=='.';) p++; + if (!(p = strchr(buff,':')) || !(p = strchr(p+1,':'))) return NULL; + for (p++;isdigit((int)*p) || *p == '.';) p++; return p+len; } - if (opt->posf==SOLF_GSIF) + if (opt->posf == SOLF_GSIF) { if (sscanf(buff,"%lf %lf %lf %lf:%lf:%lf",v,v+1,v+2,v+3,v+4,v+5)<6) { return NULL; } - *time=timeadd(epoch2time(v),-12.0*3600.0); - if (!(p=strchr(buff,':'))||!(p=strchr(p+1,':'))) return NULL; - for (p++;isdigit((int)*p)||*p=='.';) p++; + *time = timeadd(epoch2time(v),-12.0*3600.0); + if (!(p = strchr(buff,':')) || !(p = strchr(p+1,':'))) return NULL; + for (p++;isdigit((int)*p) || *p == '.';) p++; return p+len; } /* wwww ssss */ - for (p=buff,n=0;n<2;p=q+len) + for (p = buff,n = 0;n<2;p = q+len) { - if ((q=strstr(p,s))) *q='\0'; - if (*p) v[n++]=atof(p); + if ((q = strstr(p,s))) *q = '\0'; + if (*p) v[n++] = atof(p); if (!q) break; } - if (n>=2&&0.0<=v[0]&&v[0]<=3000.0&&0.0<=v[1]&&v[1]<604800.0) + if (n >= 2 && 0.0 <= v[0] && v[0] <= 3000.0 && 0.0 <= v[1] && v[1]<604800.0) { - *time=gpst2time((int)v[0],v[1]); + *time = gpst2time((int)v[0],v[1]); return p; } return NULL; @@ -360,39 +361,39 @@ char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time) /* decode x/y/z-ecef ---------------------------------------------------------*/ int decode_solxyz(char *buff, const solopt_t *opt, sol_t *sol) { - double val[MAXFIELD],P[9]={0}; - int i=0,j,n; - const char *sep=opt2sep(opt); + double val[MAXFIELD],P[9] = {0}; + int i = 0,j,n; + const char *sep = opt2sep(opt); trace(4,"decode_solxyz:\n"); - if ((n=tonum(buff,sep,val))<3) return 0; + if ((n = tonum(buff,sep,val))<3) return 0; - for (j=0;j<3;j++) + for (j = 0;j<3;j++) { - sol->rr[j]=val[i++]; /* xyz */ + sol->rr[j] = val[i++]; /* xyz */ } - if (istat=(unsigned char)val[i++]; - if (ins =(unsigned char)val[i++]; + if (istat = (unsigned char)val[i++]; + if (ins = (unsigned char)val[i++]; if (i+3age =(float)val[i++]; - if (iratio=(float)val[i]; + if (iage = (float)val[i++]; + if (iratio = (float)val[i]; - sol->type=0; /* postion type = xyz */ + sol->type = 0; /* postion type = xyz */ - if (MAXSOLQstat) sol->stat=SOLQ_NONE; + if (MAXSOLQstat) sol->stat = SOLQ_NONE; return 1; } @@ -400,52 +401,52 @@ int decode_solxyz(char *buff, const solopt_t *opt, sol_t *sol) /* decode lat/lon/height -----------------------------------------------------*/ int decode_solllh(char *buff, const solopt_t *opt, sol_t *sol) { - double val[MAXFIELD],pos[3],Q[9]={0},P[9]; - int i=0,n; - const char *sep=opt2sep(opt); + double val[MAXFIELD],pos[3],Q[9] = {0},P[9]; + int i = 0,n; + const char *sep = opt2sep(opt); trace(4,"decode_solllh:\n"); - n=tonum(buff,sep,val); + n = tonum(buff,sep,val); if (!opt->degf) { if (n<3) return 0; - pos[0]=val[i++]*D2R; /* lat/lon/hgt (ddd.ddd) */ - pos[1]=val[i++]*D2R; - pos[2]=val[i++]; + pos[0] = val[i++]*D2R; /* lat/lon/hgt (ddd.ddd) */ + pos[1] = val[i++]*D2R; + pos[2] = val[i++]; } else { if (n<7) return 0; - pos[0]=dms2deg(val )*D2R; /* lat/lon/hgt (ddd mm ss) */ - pos[1]=dms2deg(val+3)*D2R; - pos[2]=val[6]; - i+=7; + pos[0] = dms2deg(val )*D2R; /* lat/lon/hgt (ddd mm ss) */ + pos[1] = dms2deg(val+3)*D2R; + pos[2] = val[6]; + i += 7; } pos2ecef(pos,sol->rr); - if (istat=(unsigned char)val[i++]; - if (ins =(unsigned char)val[i++]; + if (istat = (unsigned char)val[i++]; + if (ins = (unsigned char)val[i++]; if (i+3age =(float)val[i++]; - if (iratio=(float)val[i]; + if (iage = (float)val[i++]; + if (iratio = (float)val[i]; - sol->type=0; /* postion type = xyz */ + sol->type = 0; /* postion type = xyz */ - if (MAXSOLQstat) sol->stat=SOLQ_NONE; + if (MAXSOLQstat) sol->stat = SOLQ_NONE; return 1; } @@ -453,39 +454,39 @@ int decode_solllh(char *buff, const solopt_t *opt, sol_t *sol) /* decode e/n/u-baseline -----------------------------------------------------*/ int decode_solenu(char *buff, const solopt_t *opt, sol_t *sol) { - double val[MAXFIELD],Q[9]={0}; - int i=0,j,n; - const char *sep=opt2sep(opt); + double val[MAXFIELD],Q[9] = {0}; + int i = 0,j,n; + const char *sep = opt2sep(opt); trace(4,"decode_solenu:\n"); - if ((n=tonum(buff,sep,val))<3) return 0; + if ((n = tonum(buff,sep,val))<3) return 0; - for (j=0;j<3;j++) + for (j = 0;j<3;j++) { - sol->rr[j]=val[i++]; /* enu */ + sol->rr[j] = val[i++]; /* enu */ } - if (istat=(unsigned char)val[i++]; - if (ins =(unsigned char)val[i++]; + if (istat = (unsigned char)val[i++]; + if (ins = (unsigned char)val[i++]; if (i+3age =(float)val[i++]; - if (iratio=(float)val[i]; + if (iage = (float)val[i++]; + if (iratio = (float)val[i]; - sol->type=1; /* postion type = enu */ + sol->type = 1; /* postion type = enu */ - if (MAXSOLQstat) sol->stat=SOLQ_NONE; + if (MAXSOLQstat) sol->stat = SOLQ_NONE; return 1; } @@ -494,17 +495,17 @@ int decode_solenu(char *buff, const solopt_t *opt, sol_t *sol) int decode_solgsi(char *buff, const solopt_t *opt, sol_t *sol) { double val[MAXFIELD]; - int i=0,j; + int i = 0,j; trace(4,"decode_solgsi:\n"); if (tonum(buff," ",val)<3) return 0; - for (j=0;j<3;j++) + for (j = 0;j<3;j++) { - sol->rr[j]=val[i++]; /* xyz */ + sol->rr[j] = val[i++]; /* xyz */ } - sol->stat=SOLQ_FIX; + sol->stat = SOLQ_FIX; return 1; } @@ -512,15 +513,15 @@ int decode_solgsi(char *buff, const solopt_t *opt, sol_t *sol) /* decode solution position --------------------------------------------------*/ int decode_solpos(char *buff, const solopt_t *opt, sol_t *sol) { - sol_t sol0={{0}}; - char *p=buff; + sol_t sol0 = {{0}}; + char *p = buff; trace(4,"decode_solpos: buff=%s\n",buff); - *sol=sol0; + *sol = sol0; /* decode solution time */ - if (!(p=decode_soltime(p,opt,&sol->time))) + if (!(p = decode_soltime(p,opt,&sol->time))) { return 0; } @@ -541,28 +542,28 @@ void decode_refpos(char *buff, const solopt_t *opt, double *rb) { double val[MAXFIELD],pos[3]; int i,n; - const char *sep=opt2sep(opt); + const char *sep = opt2sep(opt); trace(3,"decode_refpos: buff=%s\n",buff); - if ((n=tonum(buff,sep,val))<3) return; + if ((n = tonum(buff,sep,val))<3) return; - if (opt->posf==SOLF_XYZ) + if (opt->posf == SOLF_XYZ) { /* xyz */ - for (i=0;i<3;i++) rb[i]=val[i]; + for (i = 0;i<3;i++) rb[i] = val[i]; } - else if (opt->degf==0) + else if (opt->degf == 0) { /* lat/lon/hgt (ddd.ddd) */ - pos[0]=val[0]*D2R; - pos[1]=val[1]*D2R; - pos[2]=val[2]; + pos[0] = val[0]*D2R; + pos[1] = val[1]*D2R; + pos[2] = val[2]; pos2ecef(pos,rb); } - else if (opt->degf==1&&n>=7) + else if (opt->degf == 1 && n >= 7) { /* lat/lon/hgt (ddd mm ss) */ - pos[0]=dms2deg(val )*D2R; - pos[1]=dms2deg(val+3)*D2R; - pos[2]=val[6]; + pos[0] = dms2deg(val )*D2R; + pos[1] = dms2deg(val+3)*D2R; + pos[2] = val[6]; pos2ecef(pos,rb); } } @@ -577,8 +578,8 @@ int decode_sol(char *buff, const solopt_t *opt, sol_t *sol, double *rb) if (!strncmp(buff,COMMENTH,1)) { /* reference position */ - if (!strstr(buff,"ref pos")&&!strstr(buff,"slave pos")) return 0; - if (!(p=strchr(buff,':'))) return 0; + if (!strstr(buff,"ref pos") && !strstr(buff,"slave pos")) return 0; + if (!(p = strchr(buff,':'))) return 0; decode_refpos(p+1,opt,rb); return 0; } @@ -587,7 +588,7 @@ int decode_sol(char *buff, const solopt_t *opt, sol_t *sol, double *rb) if (!decode_nmea(buff,sol)) return 0; /* for time update only */ - if (opt->posf!=SOLF_NMEA&&!strncmp(buff,"$GPRMC",6)) return 2; + if (opt->posf != SOLF_NMEA && !strncmp(buff,"$GPRMC",6)) return 2; } else { /* decode position record */ @@ -604,45 +605,45 @@ void decode_solopt(char *buff, solopt_t *opt) trace(4,"decode_solhead: buff=%s\n",buff); - if (strncmp(buff,COMMENTH,1)&&strncmp(buff,"+",1)) return; + if (strncmp(buff,COMMENTH,1) && strncmp(buff,"+",1)) return; - if (strstr(buff,"GPST")) opt->times=TIMES_GPST; - else if (strstr(buff,"UTC" )) opt->times=TIMES_UTC; - else if (strstr(buff,"JST" )) opt->times=TIMES_JST; + if (strstr(buff,"GPST")) opt->times = TIMES_GPST; + else if (strstr(buff,"UTC" )) opt->times = TIMES_UTC; + else if (strstr(buff,"JST" )) opt->times = TIMES_JST; - if ((p=strstr(buff,"x-ecef(m)"))) + if ((p = strstr(buff,"x-ecef(m)"))) { - opt->posf=SOLF_XYZ; - opt->degf=0; + opt->posf = SOLF_XYZ; + opt->degf = 0; strncpy(opt->sep,p+9,1); - opt->sep[1]='\0'; + opt->sep[1] = '\0'; } - else if ((p=strstr(buff,"latitude(d'\")"))) + else if ((p = strstr(buff,"latitude(d'\")"))) { - opt->posf=SOLF_LLH; - opt->degf=1; + opt->posf = SOLF_LLH; + opt->degf = 1; strncpy(opt->sep,p+14,1); - opt->sep[1]='\0'; + opt->sep[1] = '\0'; } - else if ((p=strstr(buff,"latitude(deg)"))) + else if ((p = strstr(buff,"latitude(deg)"))) { - opt->posf=SOLF_LLH; - opt->degf=0; + opt->posf = SOLF_LLH; + opt->degf = 0; strncpy(opt->sep,p+13,1); - opt->sep[1]='\0'; + opt->sep[1] = '\0'; } - else if ((p=strstr(buff,"e-baseline(m)"))) + else if ((p = strstr(buff,"e-baseline(m)"))) { - opt->posf=SOLF_ENU; - opt->degf=0; + opt->posf = SOLF_ENU; + opt->degf = 0; strncpy(opt->sep,p+13,1); - opt->sep[1]='\0'; + opt->sep[1] = '\0'; } - else if ((p=strstr(buff,"+SITE/INF"))) + else if ((p = strstr(buff,"+SITE/INF"))) { /* gsi f2/f3 solution */ - opt->times=TIMES_GPST; - opt->posf=SOLF_GSIF; - opt->degf=0; + opt->times = TIMES_GPST; + opt->posf = SOLF_GSIF; + opt->degf = 0; strcpy(opt->sep," "); } } @@ -656,7 +657,7 @@ void readsolopt(FILE *fp, solopt_t *opt) trace(3,"readsolopt:\n"); - for (i=0;fgets(buff,sizeof(buff),fp)&&i<100;i++) + for (i = 0;fgets(buff,sizeof(buff),fp) && i<100;i++) { /* only 100 lines */ /* decode solution options */ decode_solopt(buff,opt); @@ -667,8 +668,8 @@ void readsolopt(FILE *fp, solopt_t *opt) /* input solution data from stream --------------------------------------------- * input solution data from stream * args : unsigned char data I stream data -* gtime_t ts I start time (ts.time==0: from start) -* gtime_t te I end time (te.time==0: to end) +* gtime_t ts I start time (ts.time == 0: from start) +* gtime_t te I end time (te.time == 0: to end) * double tint I time interval (0: all) * int qflag I quality flag (0: all) * solbuf_t *solbuf IO solution buffer @@ -677,22 +678,22 @@ void readsolopt(FILE *fp, solopt_t *opt) int inputsol(unsigned char data, gtime_t ts, gtime_t te, double tint, int qflag, const solopt_t *opt, solbuf_t *solbuf) { - sol_t sol={{0}}; + sol_t sol = {{0}}; int stat; trace(4,"inputsol: data=0x%02x\n",data); - sol.time=solbuf->time; + sol.time = solbuf->time; - if (data=='$'||(!isprint(data)&&data!='\r'&&data!='\n')) + if (data == '$' || (!isprint(data) && data != '\r' && data != '\n')) { /* sync header */ - solbuf->nb=0; + solbuf->nb = 0; } - solbuf->buff[solbuf->nb++]=data; - if (data!='\n'&&solbuf->nbbuff[solbuf->nb++] = data; + if (data != '\n' && solbuf->nbbuff[solbuf->nb]='\0'; - solbuf->nb=0; + solbuf->buff[solbuf->nb] = '\0'; + solbuf->nb = 0; /* check disconnect message */ if (!strcmp((char *)solbuf->buff,MSG_DISCONN)) @@ -701,11 +702,11 @@ int inputsol(unsigned char data, gtime_t ts, gtime_t te, double tint, return -1; } /* decode solution */ - if ((stat=decode_sol((char *)solbuf->buff,opt,&sol,solbuf->rb))>0) + if ((stat = decode_sol((char *)solbuf->buff,opt,&sol,solbuf->rb))>0) { - solbuf->time=sol.time; /* update current time */ + solbuf->time = sol.time; /* update current time */ } - if (stat!=1||!screent(sol.time,ts,te,tint)||(qflag&&sol.stat!=qflag)) + if (stat != 1 || !screent(sol.time,ts,te,tint) || (qflag && sol.stat != qflag)) { return 0; } @@ -722,7 +723,7 @@ int readsoldata(FILE *fp, gtime_t ts, gtime_t te, double tint, int qflag, trace(3,"readsoldata:\n"); - while ((c=fgetc(fp))!=EOF) + while ((c = fgetc(fp)) != EOF) { /* input solution */ inputsol((unsigned char)c,ts,te,tint,qflag,opt,solbuf); @@ -734,8 +735,8 @@ int readsoldata(FILE *fp, gtime_t ts, gtime_t te, double tint, int qflag, /* compare solution data -----------------------------------------------------*/ int cmpsol(const void *p1, const void *p2) { - sol_t *q1=(sol_t *)p1,*q2=(sol_t *)p2; - double tt=timediff(q1->time,q2->time); + sol_t *q1 = (sol_t *)p1,*q2 = (sol_t *)p2; + double tt = timediff(q1->time,q2->time); return tt<-0.0?-1:(tt>0.0?1:0); } @@ -747,19 +748,19 @@ int sort_solbuf(solbuf_t *solbuf) trace(4,"sort_solbuf: n=%d\n",solbuf->n); - if (solbuf->n<=0) return 0; + if (solbuf->n <= 0) return 0; - if (!(solbuf_data=(sol_t *)realloc(solbuf->data,sizeof(sol_t)*solbuf->n))) + if (!(solbuf_data = (sol_t *)realloc(solbuf->data,sizeof(sol_t)*solbuf->n))) { trace(1,"sort_solbuf: memory allocation error\n"); - free(solbuf->data); solbuf->data=NULL; solbuf->n=solbuf->nmax=0; + free(solbuf->data); solbuf->data = NULL; solbuf->n = solbuf->nmax = 0; return 0; } - solbuf->data=solbuf_data; + solbuf->data = solbuf_data; qsort(solbuf->data,solbuf->n,sizeof(sol_t),cmpsol); - solbuf->nmax=solbuf->n; - solbuf->start=0; - solbuf->end=solbuf->n-1; + solbuf->nmax = solbuf->n; + solbuf->start = 0; + solbuf->end = solbuf->n-1; return 1; } @@ -768,8 +769,8 @@ int sort_solbuf(solbuf_t *solbuf) * read solution data from soluiton files * args : char *files[] I solution files * int nfile I number of files -* (gtime_t ts) I start time (ts.time==0: from start) -* (gtime_t te) I end time (te.time==0: to end) +* (gtime_t ts) I start time (ts.time == 0: from start) +* (gtime_t te) I end time (te.time == 0: to end) * (double tint) I time interval (0: all) * (int qflag) I quality flag (0: all) * solbuf_t *solbuf O solution buffer @@ -779,16 +780,16 @@ int readsolt(char *files[], int nfile, gtime_t ts, gtime_t te, double tint, int qflag, solbuf_t *solbuf) { FILE *fp; - solopt_t opt=solopt_default; + solopt_t opt = solopt_default; int i; trace(3,"readsolt: nfile=%d\n",nfile); initsolbuf(solbuf,0,0); - for (i=0;icyclic) { /* ring buffer */ - if (solbuf->nmax<=1) return 0; - solbuf->data[solbuf->end]=*sol; - if (++solbuf->end>=solbuf->nmax) solbuf->end=0; - if (solbuf->start==solbuf->end) + if (solbuf->nmax <= 1) return 0; + solbuf->data[solbuf->end] = *sol; + if (++solbuf->end >= solbuf->nmax) solbuf->end = 0; + if (solbuf->start == solbuf->end) { - if (++solbuf->start>=solbuf->nmax) solbuf->start=0; + if (++solbuf->start >= solbuf->nmax) solbuf->start = 0; } else solbuf->n++; return 1; } - if (solbuf->n>=solbuf->nmax) + if (solbuf->n >= solbuf->nmax) { - solbuf->nmax=solbuf->nmax==0?8192:solbuf->nmax*2; - if (!(solbuf_data=(sol_t *)realloc(solbuf->data,sizeof(sol_t)*solbuf->nmax))) + solbuf->nmax = solbuf->nmax == 0?8192:solbuf->nmax*2; + if (!(solbuf_data = (sol_t *)realloc(solbuf->data,sizeof(sol_t)*solbuf->nmax))) { trace(1,"addsol: memory allocation error\n"); - free(solbuf->data); solbuf->data=NULL; solbuf->n=solbuf->nmax=0; + free(solbuf->data); solbuf->data = NULL; solbuf->n = solbuf->nmax = 0; return 0; } - solbuf->data=solbuf_data; + solbuf->data = solbuf_data; } - solbuf->data[solbuf->n++]=*sol; + solbuf->data[solbuf->n++] = *sol; return 1; } @@ -870,10 +871,10 @@ sol_t *getsol(solbuf_t *solbuf, int index) { trace(4,"getsol: index=%d\n",index); - if (index<0||solbuf->n<=index) return NULL; - if ((index=solbuf->start+index)>=solbuf->nmax) + if (index<0 || solbuf->n <= index) return NULL; + if ((index = solbuf->start+index) >= solbuf->nmax) { - index-=solbuf->nmax; + index -= solbuf->nmax; } return solbuf->data+index; } @@ -888,23 +889,23 @@ sol_t *getsol(solbuf_t *solbuf, int index) *-----------------------------------------------------------------------------*/ void initsolbuf(solbuf_t *solbuf, int cyclic, int nmax) { - gtime_t time0={0}; + gtime_t time0 = {0}; trace(3,"initsolbuf: cyclic=%d nmax=%d\n",cyclic,nmax); - solbuf->n=solbuf->nmax=solbuf->start=solbuf->end=0; - solbuf->cyclic=cyclic; - solbuf->time=time0; - solbuf->data=NULL; + solbuf->n = solbuf->nmax = solbuf->start = solbuf->end = 0; + solbuf->cyclic = cyclic; + solbuf->time = time0; + solbuf->data = NULL; if (cyclic) { - if (nmax<=2) nmax=2; - if (!(solbuf->data=malloc(sizeof(sol_t)*nmax))) + if (nmax <= 2) nmax=2; + if (!(solbuf->data = (sol_t*)malloc(sizeof(sol_t)*nmax))) { trace(1,"initsolbuf: memory allocation error\n"); return; } - solbuf->nmax=nmax; + solbuf->nmax = nmax; } } @@ -919,8 +920,8 @@ void freesolbuf(solbuf_t *solbuf) trace(3,"freesolbuf: n=%d\n",solbuf->n); free(solbuf->data); - solbuf->n=solbuf->nmax=solbuf->start=solbuf->end=0; - solbuf->data=NULL; + solbuf->n = solbuf->nmax = solbuf->start = solbuf->end = 0; + solbuf->data = NULL; } @@ -928,17 +929,17 @@ void freesolstatbuf(solstatbuf_t *solstatbuf) { trace(3,"freesolstatbuf: n=%d\n",solstatbuf->n); - solstatbuf->n=solstatbuf->nmax=0; + solstatbuf->n = solstatbuf->nmax = 0; free(solstatbuf->data); - solstatbuf->data=NULL; + solstatbuf->data = NULL; } /* compare solution status ---------------------------------------------------*/ int cmpsolstat(const void *p1, const void *p2) { - solstat_t *q1=(solstat_t *)p1,*q2=(solstat_t *)p2; - double tt=timediff(q1->time,q2->time); + solstat_t *q1 = (solstat_t *)p1,*q2 = (solstat_t *)p2; + double tt = timediff(q1->time,q2->time); return tt<-0.0?-1:(tt>0.0?1:0); } @@ -950,16 +951,17 @@ int sort_solstat(solstatbuf_t *statbuf) trace(4,"sort_solstat: n=%d\n",statbuf->n); - if (statbuf->n<=0) return 0; + if (statbuf->n <= 0) return 0; - if (!(statbuf_data=realloc(statbuf->data,sizeof(solstat_t)*statbuf->n))) { + if (!(statbuf_data = (solstat_t*)realloc(statbuf->data,sizeof(solstat_t)*statbuf->n))) + { trace(1,"sort_solstat: memory allocation error\n"); - free(statbuf->data); statbuf->data=NULL; statbuf->n=statbuf->nmax=0; + free(statbuf->data); statbuf->data = NULL; statbuf->n = statbuf->nmax = 0; return 0; } - statbuf->data=statbuf_data; + statbuf->data = statbuf_data; qsort(statbuf->data,statbuf->n,sizeof(solstat_t),cmpsolstat); - statbuf->nmax=statbuf->n; + statbuf->nmax = statbuf->n; return 1; } @@ -967,18 +969,18 @@ int sort_solstat(solstatbuf_t *statbuf) /* decode solution status ----------------------------------------------------*/ int decode_solstat(char *buff, solstat_t *stat) { - static const solstat_t stat0={{0}}; + static const solstat_t stat0 = {{0}}; double tow,az,el,resp,resc; int n,week,sat,frq,vsat,snr,fix,slip,lock,outc,slipc,rejc; - char id[32]="",*p; + char id[32] = "",*p; trace(4,"decode_solstat: buff=%s\n",buff); - if (strstr(buff,"$SAT")!=buff) return 0; + if (strstr(buff,"$SAT") != buff) return 0; - for (p=buff;*p;p++) if (*p==',') *p=' '; + for (p = buff;*p;p++) if (*p == ',') *p = ' '; - n=sscanf(buff,"$SAT%d%lf%s%d%lf%lf%lf%lf%d%d%d%d%d%d%d%d", + n = sscanf(buff,"$SAT%d%lf%s%d%lf%lf%lf%lf%d%d%d%d%d%d%d%d", &week,&tow,id,&frq,&az,&el,&resp,&resc,&vsat,&snr,&fix,&slip, &lock,&outc,&slipc,&rejc); @@ -987,25 +989,25 @@ int decode_solstat(char *buff, solstat_t *stat) trace(2,"invalid format of solution status: %s\n",buff); return 0; } - if ((sat=satid2no(id))<=0) + if ((sat = satid2no(id)) <= 0) { trace(2,"invalid satellite in solution status: %s\n",id); return 0; } - *stat=stat0; - stat->time=gpst2time(week,tow); - stat->sat =(unsigned char)sat; - stat->frq =(unsigned char)frq; - stat->az =(float)(az*D2R); - stat->el =(float)(el*D2R); - stat->resp =(float)resp; - stat->resc =(float)resc; - stat->flag =(unsigned char)((vsat<<5)+(slip<<3)+fix); - stat->snr =(unsigned char)(snr*4.0+0.5); - stat->lock =(unsigned short)lock; - stat->outc =(unsigned short)outc; - stat->slipc=(unsigned short)slipc; - stat->rejc =(unsigned short)rejc; + *stat = stat0; + stat->time = gpst2time(week,tow); + stat->sat = (unsigned char)sat; + stat->frq = (unsigned char)frq; + stat->az = (float)(az*D2R); + stat->el = (float)(el*D2R); + stat->resp = (float)resp; + stat->resc = (float)resc; + stat->flag = (unsigned char)((vsat<<5)+(slip<<3)+fix); + stat->snr = (unsigned char)(snr*4.0+0.5); + stat->lock = (unsigned short)lock; + stat->outc = (unsigned short)outc; + stat->slipc = (unsigned short)slipc; + stat->rejc = (unsigned short)rejc; return 1; } @@ -1016,19 +1018,19 @@ void addsolstat(solstatbuf_t *statbuf, const solstat_t *stat) trace(4,"addsolstat:\n"); - if (statbuf->n>=statbuf->nmax) + if (statbuf->n >= statbuf->nmax) { - statbuf->nmax=statbuf->nmax==0?8192:statbuf->nmax*2; - if (!(statbuf_data=(solstat_t *)realloc(statbuf->data,sizeof(solstat_t)* + statbuf->nmax = statbuf->nmax == 0?8192:statbuf->nmax*2; + if (!(statbuf_data = (solstat_t *)realloc(statbuf->data,sizeof(solstat_t)* statbuf->nmax))) { trace(1,"addsolstat: memory allocation error\n"); - free(statbuf->data); statbuf->data=NULL; statbuf->n=statbuf->nmax=0; + free(statbuf->data); statbuf->data = NULL; statbuf->n = statbuf->nmax = 0; return; } - statbuf->data=statbuf_data; + statbuf->data = statbuf_data; } - statbuf->data[statbuf->n++]=*stat; + statbuf->data[statbuf->n++] = *stat; } @@ -1036,7 +1038,7 @@ void addsolstat(solstatbuf_t *statbuf, const solstat_t *stat) int readsolstatdata(FILE *fp, gtime_t ts, gtime_t te, double tint, solstatbuf_t *statbuf) { - solstat_t stat={{0}}; + solstat_t stat = {{0}}; char buff[MAXSOLMSG+1]; trace(3,"readsolstatdata:\n"); @@ -1060,8 +1062,8 @@ int readsolstatdata(FILE *fp, gtime_t ts, gtime_t te, double tint, * read solution status from solution status files * args : char *files[] I solution status files * int nfile I number of files -* (gtime_t ts) I start time (ts.time==0: from start) -* (gtime_t te) I end time (te.time==0: to end) +* (gtime_t ts) I start time (ts.time == 0: from start) +* (gtime_t te) I end time (te.time == 0: to end) * (double tint) I time interval (0: all) * solstatbuf_t *statbuf O solution status buffer * return : status (1:ok,0:no data or error) @@ -1075,13 +1077,13 @@ int readsolstatt(char *files[], int nfile, gtime_t ts, gtime_t te, trace(3,"readsolstatt: nfile=%d\n",nfile); - statbuf->n=statbuf->nmax=0; - statbuf->data=NULL; + statbuf->n = statbuf->nmax = 0; + statbuf->data = NULL; - for (i=0;irr[0],sep,sol->rr[1],sep,sol->rr[2],sep,sol->stat,sep, sol->ns,sep,SQRT_SOL(sol->qr[0]),sep,SQRT_SOL(sol->qr[1]),sep,SQRT_SOL(sol->qr[2]), sep,sqvar(sol->qr[3]),sep,sqvar(sol->qr[4]),sep,sqvar(sol->qr[5]), @@ -1130,28 +1132,28 @@ int outpos(unsigned char *buff, const char *s, const sol_t *sol, const solopt_t *opt) { double pos[3],dms1[3],dms2[3],P[9],Q[9]; - const char *sep=opt2sep(opt); - char *p=(char *)buff; + const char *sep = opt2sep(opt); + char *p = (char *)buff; trace(3,"outpos :\n"); ecef2pos(sol->rr,pos); soltocov(sol,P); covenu(pos,P,Q); - if (opt->height==1) + if (opt->height == 1) { /* geodetic height */ - pos[2]-=geoidh(pos); + // pos[2] -= geoidh(pos); } if (opt->degf) { deg2dms(pos[0]*R2D,dms1); deg2dms(pos[1]*R2D,dms2); - p+=sprintf(p,"%s%s%4.0f%s%02.0f%s%08.5f%s%4.0f%s%02.0f%s%08.5f",s,sep, + p += sprintf(p,"%s%s%4.0f%s%02.0f%s%08.5f%s%4.0f%s%02.0f%s%08.5f",s,sep, dms1[0],sep,dms1[1],sep,dms1[2],sep,dms2[0],sep,dms2[1],sep, dms2[2]); } - else p+=sprintf(p,"%s%s%14.9f%s%14.9f",s,sep,pos[0]*R2D,sep,pos[1]*R2D); - p+=sprintf(p,"%s%10.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f\n", + else p += sprintf(p,"%s%s%14.9f%s%14.9f",s,sep,pos[0]*R2D,sep,pos[1]*R2D); + p += sprintf(p,"%s%10.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f\n", sep,pos[2],sep,sol->stat,sep,sol->ns,sep,SQRT_SOL(Q[4]),sep, SQRT_SOL(Q[0]),sep,SQRT_SOL(Q[8]),sep,sqvar(Q[1]),sep,sqvar(Q[2]), sep,sqvar(Q[5]),sep,sol->age,sep,sol->ratio); @@ -1165,17 +1167,17 @@ int outenu(unsigned char *buff, const char *s, const sol_t *sol, { double pos[3],rr[3],enu[3],P[9],Q[9]; int i; - const char *sep=opt2sep(opt); - char *p=(char *)buff; + const char *sep = opt2sep(opt); + char *p = (char *)buff; trace(3,"outenu :\n"); - for (i=0;i<3;i++) rr[i]=sol->rr[i]-rb[i]; + for (i = 0;i<3;i++) rr[i] = sol->rr[i]-rb[i]; ecef2pos(rb,pos); soltocov(sol,P); covenu(pos,P,Q); ecef2enu(pos,rr,enu); - p+=sprintf(p,"%s%s%14.4f%s%14.4f%s%14.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f\n", + p += sprintf(p,"%s%s%14.4f%s%14.4f%s%14.4f%s%3d%s%3d%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%8.4f%s%6.2f%s%6.1f\n", s,sep,enu[0],sep,enu[1],sep,enu[2],sep,sol->stat,sep,sol->ns,sep, SQRT_SOL(Q[0]),sep,SQRT_SOL(Q[4]),sep,SQRT_SOL(Q[8]),sep,sqvar(Q[1]), sep,sqvar(Q[5]),sep,sqvar(Q[2]),sep,sol->age,sep,sol->ratio); @@ -1186,42 +1188,42 @@ int outenu(unsigned char *buff, const char *s, const sol_t *sol, /* output solution in the form of nmea RMC sentence --------------------------*/ int outnmea_rmc(unsigned char *buff, const sol_t *sol) { - static double dirp=0.0; + static double dirp = 0.0; gtime_t time; - double ep[6],pos[3],enuv[3],dms1[3],dms2[3],vel,dir,amag=0.0; - char *p=(char *)buff,*q,sum,*emag="E"; + double ep[6],pos[3],enuv[3],dms1[3],dms2[3],vel,dir,amag = 0.0; + char *p = (char *)buff,*q,sum,*emag = "E"; trace(3,"outnmea_rmc:\n"); - if (sol->stat<=SOLQ_NONE) + if (sol->stat <= SOLQ_NONE) { - p+=sprintf(p,"$GPRMC,,,,,,,,,,,,"); - for (q=(char *)buff+1,sum=0;*q;q++) sum^=*q; - p+=sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); + p += sprintf(p,"$GPRMC,,,,,,,,,,,,"); + for (q=(char *)buff+1,sum = 0;*q;q++) sum ^= *q; + p += sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); return p-(char *)buff; } - time=gpst2utc(sol->time); - if (time.sec>=0.995) {time.time++; time.sec=0.0;} + time = gpst2utc(sol->time); + if (time.sec >= 0.995) {time.time++; time.sec = 0.0;} time2epoch(time,ep); ecef2pos(sol->rr,pos); ecef2enu(pos,sol->rr+3,enuv); - vel=norm(enuv,3); - if (vel>=1.0) + vel = norm_rtk(enuv,3); + if (vel >= 1.0) { - dir=atan2(enuv[0],enuv[1])*R2D; - if (dir<0.0) dir+=360.0; - dirp=dir; + dir = atan2(enuv[0],enuv[1])*R2D; + if (dir<0.0) dir += 360.0; + dirp = dir; } - else dir=dirp; + else dir = dirp; deg2dms(fabs(pos[0])*R2D,dms1); deg2dms(fabs(pos[1])*R2D,dms2); - p+=sprintf(p,"$GPRMC,%02.0f%02.0f%05.2f,A,%02.0f%010.7f,%s,%03.0f%010.7f,%s,%4.2f,%4.2f,%02.0f%02.0f%02d,%.1f,%s,%s", - ep[3],ep[4],ep[5],dms1[0],dms1[1]+dms1[2]/60.0,pos[0]>=0?"N":"S", - dms2[0],dms2[1]+dms2[2]/60.0,pos[1]>=0?"E":"W",vel/KNOT2M,dir, + p += sprintf(p,"$GPRMC,%02.0f%02.0f%05.2f,A,%02.0f%010.7f,%s,%03.0f%010.7f,%s,%4.2f,%4.2f,%02.0f%02.0f%02d,%.1f,%s,%s", + ep[3],ep[4],ep[5],dms1[0],dms1[1]+dms1[2]/60.0,pos[0] >= 0?"N":"S", + dms2[0],dms2[1]+dms2[2]/60.0,pos[1] >= 0?"E":"W",vel/KNOT2M,dir, ep[2],ep[1],(int)ep[0]%100,amag,emag, - sol->stat==SOLQ_DGPS||sol->stat==SOLQ_FLOAT||sol->stat==SOLQ_FIX?"D":"A"); - for (q=(char *)buff+1,sum=0;*q;q++) sum^=*q; /* check-sum */ - p+=sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); + sol->stat == SOLQ_DGPS || sol->stat == SOLQ_FLOAT || sol->stat == SOLQ_FIX?"D":"A"); + for (q = (char *)buff+1,sum = 0;*q;q++) sum ^= *q; /* check-sum */ + p += sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); return p-(char *)buff; } @@ -1230,34 +1232,34 @@ int outnmea_rmc(unsigned char *buff, const sol_t *sol) int outnmea_gga(unsigned char *buff, const sol_t *sol) { gtime_t time; - double h,ep[6],pos[3],dms1[3],dms2[3],dop=1.0; + double h,ep[6],pos[3],dms1[3],dms2[3],dop = 1.0; int solq; - char *p=(char *)buff,*q,sum; + char *p = (char *)buff,*q,sum; trace(3,"outnmea_gga:\n"); - if (sol->stat<=SOLQ_NONE) + if (sol->stat <= SOLQ_NONE) { - p+=sprintf(p,"$GPGGA,,,,,,,,,,,,,,"); - for (q=(char *)buff+1,sum=0;*q;q++) sum^=*q; - p+=sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); + p += sprintf(p,"$GPGGA,,,,,,,,,,,,,,"); + for (q = (char *)buff+1,sum = 0;*q;q++) sum ^= *q; + p += sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); return p-(char *)buff; } - for (solq=0;solq<8;solq++) if (solq_nmea[solq]==sol->stat) break; - if (solq>=8) solq=0; - time=gpst2utc(sol->time); - if (time.sec>=0.995) {time.time++; time.sec=0.0;} + for (solq = 0;solq<8;solq++) if (solq_nmea[solq] == sol->stat) break; + if (solq >= 8) solq = 0; + time = gpst2utc(sol->time); + if (time.sec >= 0.995) {time.time++; time.sec = 0.0;} time2epoch(time,ep); ecef2pos(sol->rr,pos); - h=geoidh(pos); + h = 0;//geoidh(pos); deg2dms(fabs(pos[0])*R2D,dms1); deg2dms(fabs(pos[1])*R2D,dms2); - p+=sprintf(p,"$GPGGA,%02.0f%02.0f%05.2f,%02.0f%010.7f,%s,%03.0f%010.7f,%s,%d,%02d,%.1f,%.3f,M,%.3f,M,%.1f,", - ep[3],ep[4],ep[5],dms1[0],dms1[1]+dms1[2]/60.0,pos[0]>=0?"N":"S", - dms2[0],dms2[1]+dms2[2]/60.0,pos[1]>=0?"E":"W",solq, + p += sprintf(p,"$GPGGA,%02.0f%02.0f%05.2f,%02.0f%010.7f,%s,%03.0f%010.7f,%s,%d,%02d,%.1f,%.3f,M,%.3f,M,%.1f,", + ep[3],ep[4],ep[5],dms1[0],dms1[1]+dms1[2]/60.0,pos[0] >= 0?"N":"S", + dms2[0],dms2[1]+dms2[2]/60.0,pos[1] >= 0?"E":"W",solq, sol->ns,dop,pos[2]-h,h,sol->age); - for (q=(char *)buff+1,sum=0;*q;q++) sum^=*q; /* check-sum */ - p+=sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); + for (q = (char *)buff+1,sum = 0;*q;q++) sum ^= *q; /* check-sum */ + p += sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); return p-(char *)buff; } @@ -1268,84 +1270,85 @@ int outnmea_gsa(unsigned char *buff, const sol_t *sol, { double azel[MAXSAT*2],dop[4]; int i,sat,sys,nsat,prn[MAXSAT]; - char *p=(char *)buff,*q,*s,sum; + char *p = (char *)buff,*q,*s,sum; trace(3,"outnmea_gsa:\n"); - if (sol->stat<=SOLQ_NONE) + if (sol->stat <= SOLQ_NONE) { - p+=sprintf(p,"$GPGSA,A,1,,,,,,,,,,,,,,,"); - for (q=(char *)buff+1,sum=0;*q;q++) sum^=*q; - p+=sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); + p += sprintf(p,"$GPGSA,A,1,,,,,,,,,,,,,,,"); + for (q = (char *)buff+1,sum = 0;*q;q++) sum ^= *q; + p += sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); return p-(char *)buff; } /* GPGSA: gps/sbas */ - for (sat=1,nsat=0;sat<=MAXSAT&&nsat<12;sat++) { - if (!ssat[sat-1].vs||ssat[sat-1].azel[1]<=0.0) continue; - sys=satsys(sat,prn+nsat); - if (sys!=SYS_GPS&&sys!=SYS_SBS) continue; - if (sys==SYS_SBS) prn[nsat]+=33-MINPRNSBS; - for (i=0;i<2;i++) azel[i+nsat*2]=ssat[sat-1].azel[i]; + for (sat = 1,nsat = 0;sat <= MAXSAT&&nsat<12;sat++) + { + if (!ssat[sat-1].vs || ssat[sat-1].azel[1] <= 0.0) continue; + sys = satsys(sat,prn+nsat); + if (sys != SYS_GPS && sys != SYS_SBS) continue; + if (sys == SYS_SBS) prn[nsat] += 33-MINPRNSBS; + for (i = 0;i<2;i++) azel[i+nsat*2] = ssat[sat-1].azel[i]; nsat++; } if (nsat>0) { - s=p; - p+=sprintf(p,"$GPGSA,A,%d",sol->stat<=0?1:3); - for (i=0;i<12;i++) + s = p; + p += sprintf(p,"$GPGSA,A,%d",sol->stat <= 0?1:3); + for (i = 0;i<12;i++) { - if (i0) { - s=p; - p+=sprintf(p,"$GLGSA,A,%d",sol->stat<=0?1:3); - for (i=0;i<12;i++) + s = p; + p += sprintf(p,"$GLGSA,A,%d",sol->stat <= 0?1:3); + for (i = 0;i<12;i++) { - if (i0) { - s=p; - p+=sprintf(p,"$GAGSA,A,%d",sol->stat<=0?1:3); - for (i=0;i<12;i++) + s = p; + p += sprintf(p,"$GAGSA,A,%d",sol->stat <= 0?1:3); + for (i = 0;i<12;i++) { - if (istat<=SOLQ_NONE) + if (sol->stat <= SOLQ_NONE) { - p+=sprintf(p,"$GPGSV,1,1,0,,,,,,,,,,,,,,,,"); - for (q=(char *)buff+1,sum=0;*q;q++) sum^=*q; - p+=sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); + p += sprintf(p,"$GPGSV,1,1,0,,,,,,,,,,,,,,,,"); + for (q = (char *)buff+1,sum = 0;*q;q++) sum ^= *q; + p += sprintf(p,"*%02X%c%c",sum,0x0D,0x0A); return p-(char *)buff; } /* GPGSV: gps/sbas */ - for (sat=1,n=0;sat0.0) sats[n++]=sat; + sys = satsys(sat,&prn); + if (sys != SYS_GPS && sys != SYS_SBS) continue; + if (ssat[sat-1].vs && ssat[sat-1].azel[1]>0.0) sats[n++] = sat; } - nmsg=n<=0?0:(n-1)/4+1; + nmsg = n <= 0?0:(n-1)/4+1; - for (i=k=0;i0.0) sats[n++]=sat; + if (satsys(sat,&prn) != SYS_GLO) continue; + if (ssat[sat-1].vs && ssat[sat-1].azel[1]>0.0) sats[n++] = sat; } - nmsg=n<=0?0:(n-1)/4+1; + nmsg = n <= 0?0:(n-1)/4+1; - for (i=k=0;i0.0) sats[n++]=sat; + if (satsys(sat,&prn) != SYS_GAL) continue; + if (ssat[sat-1].vs && ssat[sat-1].azel[1]>0.0) sats[n++] = sat; } - nmsg=n<=0?0:(n-1)/4+1; + nmsg = n <= 0?0:(n-1)/4+1; - for (i=k=0;imode]); + p += sprintf(p,"%s pos mode : %s\n",COMMENTH,s1[opt->mode]); - if (PMODE_DGPS<=opt->mode&&opt->mode<=PMODE_FIXED) + if (PMODE_DGPS <= opt->mode && opt->mode <= PMODE_FIXED) { - p+=sprintf(p,"%s freqs : %s\n",COMMENTH,s2[opt->nf-1]); + p += sprintf(p,"%s freqs : %s\n",COMMENTH,s2[opt->nf-1]); } if (opt->mode>PMODE_SINGLE) { - p+=sprintf(p,"%s solution : %s\n",COMMENTH,s3[opt->soltype]); + p += sprintf(p,"%s solution : %s\n",COMMENTH,s3[opt->soltype]); } - p+=sprintf(p,"%s elev mask : %.1f deg\n",COMMENTH,opt->elmin*R2D); + p += sprintf(p,"%s elev mask : %.1f deg\n",COMMENTH,opt->elmin*R2D); if (opt->mode>PMODE_SINGLE) { - p+=sprintf(p,"%s dynamics : %s\n",COMMENTH,opt->dynamics?"on":"off"); - p+=sprintf(p,"%s tidecorr : %s\n",COMMENTH,opt->tidecorr?"on":"off"); + p += sprintf(p,"%s dynamics : %s\n",COMMENTH,opt->dynamics?"on":"off"); + p += sprintf(p,"%s tidecorr : %s\n",COMMENTH,opt->tidecorr?"on":"off"); } - if (opt->mode<=PMODE_FIXED) + if (opt->mode <= PMODE_FIXED) { - p+=sprintf(p,"%s ionos opt : %s\n",COMMENTH,s4[opt->ionoopt]); + p += sprintf(p,"%s ionos opt : %s\n",COMMENTH,s4[opt->ionoopt]); } - p+=sprintf(p,"%s tropo opt : %s\n",COMMENTH,s5[opt->tropopt]); - p+=sprintf(p,"%s ephemeris : %s\n",COMMENTH,s6[opt->sateph]); - if (opt->navsys!=SYS_GPS) + p += sprintf(p,"%s tropo opt : %s\n",COMMENTH,s5[opt->tropopt]); + p += sprintf(p,"%s ephemeris : %s\n",COMMENTH,s6[opt->sateph]); + if (opt->navsys != SYS_GPS) { - p+=sprintf(p,"%s navi sys :",COMMENTH); - for (i=0;sys[i];i++) + p += sprintf(p,"%s navi sys :",COMMENTH); + for (i = 0;sys[i];i++) { - if (opt->navsys&sys[i]) p+=sprintf(p," %s",s7[i]); + if (opt->navsys&sys[i]) p += sprintf(p," %s",s7[i]); } - p+=sprintf(p,"\n"); + p += sprintf(p,"\n"); } - if (PMODE_KINEMA<=opt->mode&&opt->mode<=PMODE_FIXED) + if (PMODE_KINEMA <= opt->mode && opt->mode <= PMODE_FIXED) { - p+=sprintf(p,"%s amb res : %s\n",COMMENTH,s8[opt->modear]); + p += sprintf(p,"%s amb res : %s\n",COMMENTH,s8[opt->modear]); if (opt->navsys&SYS_GLO) { - p+=sprintf(p,"%s amb glo : %s\n",COMMENTH,s9[opt->glomodear]); + p += sprintf(p,"%s amb glo : %s\n",COMMENTH,s9[opt->glomodear]); } if (opt->thresar[0]>0.0) { - p+=sprintf(p,"%s val thres : %.1f\n",COMMENTH,opt->thresar[0]); + p += sprintf(p,"%s val thres : %.1f\n",COMMENTH,opt->thresar[0]); } } - if (opt->mode==PMODE_MOVEB&&opt->baseline[0]>0.0) + if (opt->mode == PMODE_MOVEB && opt->baseline[0]>0.0) { - p+=sprintf(p,"%s baseline : %.4f %.4f m\n",COMMENTH, + p += sprintf(p,"%s baseline : %.4f %.4f m\n",COMMENTH, opt->baseline[0],opt->baseline[1]); } - for (i=0;i<2;i++) + for (i = 0;i<2;i++) { - if (opt->mode==PMODE_SINGLE||(i>=1&&opt->mode>PMODE_FIXED)) continue; - p+=sprintf(p,"%s antenna%d : %-21s (%7.4f %7.4f %7.4f)\n",COMMENTH, + if (opt->mode == PMODE_SINGLE || (i >= 1 && opt->mode>PMODE_FIXED)) continue; + p += sprintf(p,"%s antenna%d : %-21s (%7.4f %7.4f %7.4f)\n",COMMENTH, i+1,opt->anttype[i],opt->antdel[i][0],opt->antdel[i][1], opt->antdel[i][2]); } @@ -1554,52 +1557,52 @@ int outprcopts(unsigned char *buff, const prcopt_t *opt) *-----------------------------------------------------------------------------*/ int outsolheads(unsigned char *buff, const solopt_t *opt) { - const char *s1[]={"WGS84","Tokyo"},*s2[]={"ellipsoidal","geodetic"}; - const char *s3[]={"GPST","UTC ","JST "},*sep=opt2sep(opt); - char *p=(char *)buff; - int timeu=opt->timeu<0?0:(opt->timeu>20?20:opt->timeu); + const char *s1[] = {"WGS84","Tokyo"},*s2[] = {"ellipsoidal","geodetic"}; + const char *s3[] = {"GPST","UTC ","JST "},*sep = opt2sep(opt); + char *p = (char *)buff; + int timeu = opt->timeu<0?0:(opt->timeu>20?20:opt->timeu); trace(3,"outsolheads:\n"); - if (opt->posf==SOLF_NMEA) return 0; + if (opt->posf == SOLF_NMEA) return 0; if (opt->outhead) { - p+=sprintf(p,"%s (",COMMENTH); - if (opt->posf==SOLF_XYZ) p+=sprintf(p,"x/y/z-ecef=WGS84"); - else if (opt->posf==SOLF_ENU) p+=sprintf(p,"e/n/u-baseline=WGS84"); - else p+=sprintf(p,"lat/lon/height=%s/%s",s1[opt->datum],s2[opt->height]); - p+=sprintf(p,",Q=1:fix,2:float,3:sbas,4:dgps,5:single,6:ppp,ns=# of satellites)\n"); + p += sprintf(p,"%s (",COMMENTH); + if (opt->posf == SOLF_XYZ) p += sprintf(p,"x/y/z-ecef=WGS84"); + else if (opt->posf == SOLF_ENU) p += sprintf(p,"e/n/u-baseline=WGS84"); + else p += sprintf(p,"lat/lon/height=%s/%s",s1[opt->datum],s2[opt->height]); + p += sprintf(p,",Q=1:fix,2:float,3:sbas,4:dgps,5:single,6:ppp,ns=# of satellites)\n"); } - p+=sprintf(p,"%s %-*s%s",COMMENTH,(opt->timef?16:8)+timeu+1,s3[opt->times],sep); + p += sprintf(p,"%s %-*s%s",COMMENTH,(opt->timef?16:8)+timeu+1,s3[opt->times],sep); - if (opt->posf==SOLF_LLH) + if (opt->posf == SOLF_LLH) { /* lat/lon/hgt */ if (opt->degf) { - p+=sprintf(p,"%16s%s%16s%s%10s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", + p += sprintf(p,"%16s%s%16s%s%10s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", "latitude(d'\")",sep,"longitude(d'\")",sep,"height(m)",sep, "Q",sep,"ns",sep,"sdn(m)",sep,"sde(m)",sep,"sdu(m)",sep, "sdne(m)",sep,"sdeu(m)",sep,"sdue(m)",sep,"age(s)",sep,"ratio"); } else { - p+=sprintf(p,"%14s%s%14s%s%10s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", + p += sprintf(p,"%14s%s%14s%s%10s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", "latitude(deg)",sep,"longitude(deg)",sep,"height(m)",sep, "Q",sep,"ns",sep,"sdn(m)",sep,"sde(m)",sep,"sdu(m)",sep, "sdne(m)",sep,"sdeu(m)",sep,"sdun(m)",sep,"age(s)",sep,"ratio"); } } - else if (opt->posf==SOLF_XYZ) + else if (opt->posf == SOLF_XYZ) { /* x/y/z-ecef */ - p+=sprintf(p,"%14s%s%14s%s%14s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", + p += sprintf(p,"%14s%s%14s%s%14s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", "x-ecef(m)",sep,"y-ecef(m)",sep,"z-ecef(m)",sep,"Q",sep,"ns",sep, "sdx(m)",sep,"sdy(m)",sep,"sdz(m)",sep,"sdxy(m)",sep, "sdyz(m)",sep,"sdzx(m)",sep,"age(s)",sep,"ratio"); } - else if (opt->posf==SOLF_ENU) + else if (opt->posf == SOLF_ENU) { /* e/n/u-baseline */ - p+=sprintf(p,"%14s%s%14s%s%14s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", + p += sprintf(p,"%14s%s%14s%s%14s%s%3s%s%3s%s%8s%s%8s%s%8s%s%8s%s%8s%s%8s%s%6s%s%6s\n", "e-baseline(m)",sep,"n-baseline(m)",sep,"u-baseline(m)",sep, "Q",sep,"ns",sep,"sde(m)",sep,"sdn(m)",sep,"sdu(m)",sep, "sden(m)",sep,"sdnu(m)",sep,"sdue(m)",sep,"age(s)",sep,"ratio"); @@ -1619,48 +1622,48 @@ int outsolheads(unsigned char *buff, const solopt_t *opt) int outsols(unsigned char *buff, const sol_t *sol, const double *rb, const solopt_t *opt) { - gtime_t time,ts={0}; + gtime_t time,ts = {0}; double gpst; int week,timeu; - const char *sep=opt2sep(opt); + const char *sep = opt2sep(opt); char s[64]; - unsigned char *p=buff; + unsigned char *p = buff; trace(3,"outsols :\n"); - if (opt->posf==SOLF_NMEA) + if (opt->posf == SOLF_NMEA) { if (opt->nmeaintv[0]<0.0) return 0; if (!screent(sol->time,ts,ts,opt->nmeaintv[0])) return 0; } - if (sol->stat<=SOLQ_NONE||(opt->posf==SOLF_ENU&&norm(rb,3)<=0.0)) + if (sol->stat <= SOLQ_NONE || (opt->posf == SOLF_ENU && norm_rtk(rb,3) <= 0.0)) { return 0; } - timeu=opt->timeu<0?0:(opt->timeu>20?20:opt->timeu); + timeu = opt->timeu<0?0:(opt->timeu>20?20:opt->timeu); - time=sol->time; - if (opt->times>=TIMES_UTC) time=gpst2utc(time); - if (opt->times==TIMES_JST) time=timeadd(time,9*3600.0); + time = sol->time; + if (opt->times >= TIMES_UTC) time = gpst2utc(time); + if (opt->times == TIMES_JST) time = timeadd(time,9*3600.0); if (opt->timef) time2str(time,s,timeu); else { - gpst=time2gpst(time,&week); + gpst = time2gpst(time,&week); if (86400*7-gpst<0.5/pow(10.0,timeu)) { week++; - gpst=0.0; + gpst = 0.0; } - sprintf(s,"%4d%s%*.*f",week,sep,6+(timeu<=0?0:timeu+1),timeu,gpst); + sprintf(s,"%4d%s%*.*f",week,sep,6+(timeu <= 0?0:timeu+1),timeu,gpst); } switch (opt->posf) { - case SOLF_LLH: p+=outpos (p,s,sol,opt); break; - case SOLF_XYZ: p+=outecef(p,s,sol,opt); break; - case SOLF_ENU: p+=outenu(p,s,sol,rb,opt); break; - case SOLF_NMEA: p+=outnmea_rmc(p,sol); - p+=outnmea_gga(p,sol); break; + case SOLF_LLH: p += outpos (p,s,sol,opt); break; + case SOLF_XYZ: p += outecef(p,s,sol,opt); break; + case SOLF_ENU: p += outenu(p,s,sol,rb,opt); break; + case SOLF_NMEA: p += outnmea_rmc(p,sol); + p += outnmea_gga(p,sol); break; } return p-buff; } @@ -1678,20 +1681,20 @@ int outsols(unsigned char *buff, const sol_t *sol, const double *rb, int outsolexs(unsigned char *buff, const sol_t *sol, const ssat_t *ssat, const solopt_t *opt) { - gtime_t ts={0}; - unsigned char *p=buff; + gtime_t ts = {0}; + unsigned char *p = buff; trace(3,"outsolexs:\n"); - if (opt->posf==SOLF_NMEA) + if (opt->posf == SOLF_NMEA) { if (opt->nmeaintv[1]<0.0) return 0; if (!screent(sol->time,ts,ts,opt->nmeaintv[1])) return 0; } - if (opt->posf==SOLF_NMEA) + if (opt->posf == SOLF_NMEA) { - p+=outnmea_gsa(p,sol,ssat); - p+=outnmea_gsv(p,sol,ssat); + p += outnmea_gsa(p,sol,ssat); + p += outnmea_gsv(p,sol,ssat); } return p-buff; } @@ -1710,7 +1713,7 @@ void outprcopt(FILE *fp, const prcopt_t *opt) trace(3,"outprcopt:\n"); - if ((n=outprcopts(buff,opt))>0) + if ((n = outprcopts(buff,opt))>0) { fwrite(buff,n,1,fp); } @@ -1730,7 +1733,7 @@ void outsolhead(FILE *fp, const solopt_t *opt) trace(3,"outsolhead:\n"); - if ((n=outsolheads(buff,opt))>0) + if ((n = outsolheads(buff,opt))>0) { fwrite(buff,n,1,fp); } @@ -1753,7 +1756,7 @@ void outsol(FILE *fp, const sol_t *sol, const double *rb, trace(3,"outsol :\n"); - if ((n=outsols(buff,sol,rb,opt))>0) + if ((n = outsols(buff,sol,rb,opt))>0) { fwrite(buff,n,1,fp); } @@ -1777,7 +1780,7 @@ void outsolex(FILE *fp, const sol_t *sol, const ssat_t *ssat, trace(3,"outsolex:\n"); - if ((n=outsolexs(buff,sol,ssat,opt))>0) + if ((n = outsolexs(buff,sol,ssat,opt))>0) { fwrite(buff,n,1,fp); } diff --git a/src/algorithms/libs/rtklib/rtklib_solution.h b/src/algorithms/libs/rtklib/rtklib_solution.h index 53719cd9b..c1c5b5bea 100644 --- a/src/algorithms/libs/rtklib/rtklib_solution.h +++ b/src/algorithms/libs/rtklib/rtklib_solution.h @@ -55,6 +55,9 @@ #include "rtklib.h" +#define COMMENTH "%" /* comment line indicator for solution */ +#define MSG_DISCONN "$_DISCONNECT\r\n" /* disconnect message */ + const char *opt2sep(const solopt_t *opt); int tonum(char *buff, const char *sep, double *v); double sqvar(double covar); diff --git a/src/algorithms/libs/rtklib/rtklib_stream.cc b/src/algorithms/libs/rtklib/rtklib_stream.cc index 5e0ba5c03..70ca75c34 100644 --- a/src/algorithms/libs/rtklib/rtklib_stream.cc +++ b/src/algorithms/libs/rtklib/rtklib_stream.cc @@ -108,7 +108,7 @@ } else strcpy(port,path); - for (i=0;i<11;i++) if (br[i]==brate) break; + for (i=0;i<11;i++) if (br[i] == brate) break; if (i>=12) { sprintf(msg,"bitrate error (%d)",brate); @@ -120,7 +120,7 @@ sprintf(dev,"/dev/%s",port); - if ((mode&STR_MODE_R)&&(mode&STR_MODE_W)) rw=O_RDWR; + if ((mode&STR_MODE_R) && (mode&STR_MODE_W)) rw=O_RDWR; else if (mode&STR_MODE_R) rw=O_RDONLY; else if (mode&STR_MODE_W) rw=O_WRONLY; @@ -139,10 +139,10 @@ ios.c_cc[VTIME]=0; cfsetospeed(&ios,bs[i]); cfsetispeed(&ios,bs[i]); - ios.c_cflag|=bsize==7?CS7:CS8; - ios.c_cflag|=parity=='O'?(PARENB|PARODD):(parity=='E'?PARENB:0); - ios.c_cflag|=stopb==2?CSTOPB:0; - ios.c_cflag|=!strcmp(fctr,"rts")?CRTSCTS:0; + ios.c_cflag |= bsize == 7?CS7:CS8; + ios.c_cflag |= parity == 'O'?(PARENB|PARODD):(parity == 'E'?PARENB:0); + ios.c_cflag |= stopb == 2?CSTOPB:0; + ios.c_cflag |= !strcmp(fctr,"rts")?CRTSCTS:0; tcsetattr(serial->dev,TCSANOW,&ios); tcflush(serial->dev,TCIOFLUSH); return serial; @@ -213,7 +213,7 @@ reppath(file->path,file->openpath,time,"",""); /* create directory */ - if ((file->mode&STR_MODE_W)&&!(file->mode&STR_MODE_R)) + if ((file->mode&STR_MODE_W) && !(file->mode&STR_MODE_R)) { createdir(file->openpath); } @@ -242,8 +242,8 @@ if (file->mode&STR_MODE_R) { - if (fread(&tagh,TIMETAGH_LEN,1,file->fp_tag)==1&& - fread(&file->time,sizeof(file->time),1,file->fp_tag)==1) + if (fread(&tagh,TIMETAGH_LEN,1,file->fp_tag) == 1 && + fread(&file->time,sizeof(file->time),1,file->fp_tag) == 1) { memcpy(&file->tick_f,tagh+TIMETAGH_LEN-4,sizeof(file->tick_f)); } @@ -304,12 +304,12 @@ if (!(mode&(STR_MODE_R|STR_MODE_W))) return NULL; /* file options */ - for (p=(char *)path;(p=strstr(p,"::"));p+=2) + for (p=(char *)path;(p=strstr(p,"::"));p += 2) { /* file options */ - if (*(p+2)=='T') timetag=1; - else if (*(p+2)=='+') sscanf(p+2,"+%lf",&start); - else if (*(p+2)=='x') sscanf(p+2,"x%lf",&speed); - else if (*(p+2)=='S') sscanf(p+2,"S=%lf",&swapintv); + if (*(p+2) == 'T') timetag=1; + else if (*(p+2) == '+') sscanf(p+2,"+%lf",&start); + else if (*(p+2) == 'x') sscanf(p+2,"x%lf",&speed); + else if (*(p+2) == 'S') sscanf(p+2,"S=%lf",&swapintv); } if (start<=0.0) start=0.0; if (swapintv<=0.0) swapintv=0.0; @@ -361,7 +361,7 @@ tracet(3,"swapfile: fp=%d time=%s\n",file->fp,time_str(time,0)); /* return if old swap file open */ - if (file->fp_tmp||file->fp_tag_tmp) return; + if (file->fp_tmp || file->fp_tag_tmp) return; /* check path of new swap file */ reppath(file->path,openpath,time,"",""); @@ -409,7 +409,7 @@ if (!file) return 0; - if (file->fp==stdin) + if (file->fp == stdin) { /* input from stdin */ FD_ZERO(&rs); FD_SET(0,&rs); @@ -429,14 +429,14 @@ } for (;;) { /* seek file position */ - if (fread(&tick,sizeof(tick),1,file->fp_tag)<1|| + if (fread(&tick,sizeof(tick),1,file->fp_tag)<1 || fread(&fpos,sizeof(fpos),1,file->fp_tag)<1) { fseek(file->fp,0,SEEK_END); sprintf(msg,"end"); break; } - if (file->repmode||file->speed>0.0) + if (file->repmode || file->speed>0.0) { if ((int)(tick-t)<1) continue; } @@ -452,7 +452,7 @@ } nmax=(int)(fpos-file->fpos); - if (file->repmode||file->speed>0.0) + if (file->repmode || file->speed>0.0) { fseek(file->fp_tag,-(long)(sizeof(tick)+sizeof(fpos)),SEEK_CUR); } @@ -462,7 +462,7 @@ if (nmax>0) { nr=fread(buff,1,nmax,file->fp); - file->fpos+=nr; + file->fpos += nr; if (nr<=0) sprintf(msg,"end"); } tracet(5,"readfile: fp=%d nr=%d fpos=%d\n",file->fp,nr,file->fpos); @@ -486,12 +486,12 @@ wtime=utc2gpst(timeget()); /* write time in gpst */ /* swap writing file */ - if (file->swapintv>0.0&&file->wtime.time!=0) + if (file->swapintv>0.0 && file->wtime.time!=0) { intv=file->swapintv*3600.0; tow1=time2gpst(file->wtime,&week1); tow2=time2gpst(wtime,&week2); - tow2+=604800.0*(week2-week1); + tow2 += 604800.0*(week2-week1); /* open new swap file */ if (floor((tow1+fswapmargin)/intv)fp_tag) { - tick-=file->tick; + tick -= file->tick; fwrite(&tick,1,sizeof(tick),file->fp_tag); fwrite(&fpos,1,sizeof(fpos),file->fp_tag); fflush(file->fp_tag); @@ -540,7 +540,7 @@ /* sync files by time-tag ----------------------------------------------------*/ void syncfile(file_t *file1, file_t *file2) { - if (!file1->fp_tag||!file2->fp_tag) return; + if (!file1->fp_tag || !file2->fp_tag) return; file1->repmode=0; file2->repmode=1; file2->offset=(int)(file1->tick_f-file2->tick_f); @@ -575,7 +575,7 @@ } if ((p=strrchr(buff,'@'))) { - *p++='\0'; + *p++ = '\0'; if ((q=strchr(buff,':'))) { *q='\0'; if (passwd) strcpy(passwd,q+1); @@ -604,21 +604,21 @@ tracet(3,"setsock: sock=%d\n",sock); - if (setsockopt(sock,SOL_SOCKET,SO_RCVTIMEO,(const char *)&tv,sizeof(tv))==-1|| - setsockopt(sock,SOL_SOCKET,SO_SNDTIMEO,(const char *)&tv,sizeof(tv))==-1) + if (setsockopt(sock,SOL_SOCKET,SO_RCVTIMEO,(const char *)&tv,sizeof(tv)) == -1 || + setsockopt(sock,SOL_SOCKET,SO_SNDTIMEO,(const char *)&tv,sizeof(tv)) == -1) { sprintf(msg,"sockopt error: notimeo"); tracet(1,"setsock: setsockopt error 1 sock=%d err=%d\n",sock,errsock()); closesocket(sock); return 0; } - if (setsockopt(sock,SOL_SOCKET,SO_RCVBUF,(const char *)&bs,sizeof(bs))==-1|| - setsockopt(sock,SOL_SOCKET,SO_SNDBUF,(const char *)&bs,sizeof(bs))==-1) + if (setsockopt(sock,SOL_SOCKET,SO_RCVBUF,(const char *)&bs,sizeof(bs)) == -1 || + setsockopt(sock,SOL_SOCKET,SO_SNDBUF,(const char *)&bs,sizeof(bs)) == -1) { tracet(1,"setsock: setsockopt error 2 sock=%d err=%d bs=%d\n",sock,errsock(),bs); sprintf(msg,"sockopt error: bufsiz"); } - if (setsockopt(sock,IPPROTO_TCP,TCP_NODELAY,(const char *)&mode,sizeof(mode))==-1) + if (setsockopt(sock,IPPROTO_TCP,TCP_NODELAY,(const char *)&mode,sizeof(mode)) == -1) { tracet(1,"setsock: setsockopt error 3 sock=%d err=%d\n",sock,errsock()); sprintf(msg,"sockopt error: nodelay"); @@ -647,12 +647,12 @@ flag=fcntl(sock,F_GETFL,0); fcntl(sock,F_SETFL,flag|O_NONBLOCK); - if (connect(sock,addr,len)==-1) + if (connect(sock,addr,len) == -1) { err=errsock(); - if (err!=EISCONN&&err!=EINPROGRESS&&err!=EALREADY) return -1; + if (err!=EISCONN && err!=EINPROGRESS && err!=EALREADY) return -1; FD_ZERO(&rs); FD_SET(sock,&rs); ws=rs; - if (select(sock+1,&rs,&ws,NULL,&tv)==0) return 0; + if (select(sock+1,&rs,&ws,NULL,&tv) == 0) return 0; } return 1; } @@ -691,7 +691,7 @@ tracet(3,"gentcp: type=%d\n",type); /* generate socket */ - if ((tcp->sock=socket(AF_INET,SOCK_STREAM,0))==(socket_t)-1) + if ((tcp->sock=socket(AF_INET,SOCK_STREAM,0)) == (socket_t)-1) { sprintf(msg,"socket error (%d)",errsock()); tracet(1,"gentcp: socket error err=%d\n",errsock()); @@ -707,7 +707,7 @@ tcp->addr.sin_family=AF_INET; tcp->addr.sin_port=htons(tcp->port); - if (type==0) + if (type == 0) { /* server socket */ #ifdef SVR_REUSEADDR @@ -715,7 +715,7 @@ setsockopt(tcp->sock,SOL_SOCKET,SO_REUSEADDR,(const char *)&opt, sizeof(opt)); #endif - if (bind(tcp->sock,(struct sockaddr *)&tcp->addr,sizeof(tcp->addr))==-1) + if (bind(tcp->sock,(struct sockaddr *)&tcp->addr,sizeof(tcp->addr)) == -1) { sprintf(msg,"bind error (%d) : %d",errsock(),tcp->port); tracet(1,"gentcp: bind error port=%d err=%d\n",tcp->port,errsock()); @@ -807,7 +807,7 @@ tracet(3,"updatetcpsvr: state=%d\n",tcpsvr->svr.state); - if (tcpsvr->svr.state==0) return; + if (tcpsvr->svr.state == 0) return; for (i=0;icli[i].saddr); n++; } - if (n==0) + if (n == 0) { tcpsvr->svr.state=1; sprintf(msg,"waiting..."); return; } tcpsvr->svr.state=2; - if (n==1) sprintf(msg,"%s",saddr); else sprintf(msg,"%d clients",n); + if (n == 1) sprintf(msg,"%s",saddr); else sprintf(msg,"%d clients",n); } @@ -848,10 +848,10 @@ tracet(3,"accsock: sock=%d\n",tcpsvr->svr.sock); - for (i=0;icli[i].state==0) break; + for (i=0;icli[i].state == 0) break; if (i>=MAXCLI) return 0; /* too many client */ - if ((sock=accept_nb(tcpsvr->svr.sock,(struct sockaddr *)&addr,&len))==(socket_t)-1) + if ((sock=accept_nb(tcpsvr->svr.sock,(struct sockaddr *)&addr,&len)) == (socket_t)-1) { err=errsock(); sprintf(msg,"accept error (%d)",err); @@ -859,7 +859,7 @@ closesocket(tcpsvr->svr.sock); tcpsvr->svr.state=0; return 0; } - if (sock==0) return 0; + if (sock == 0) return 0; tcpsvr->cli[i].sock=sock; if (!setsock(tcpsvr->cli[i].sock,msg)) return 0; @@ -880,7 +880,7 @@ if (tcpsvr->svr.state<=0) return 0; while (accsock(tcpsvr,msg)) ; updatetcpsvr(tcpsvr,msg); - return tcpsvr->svr.state==2; + return tcpsvr->svr.state == 2; } @@ -891,9 +891,9 @@ tracet(4,"readtcpsvr: state=%d n=%d\n",tcpsvr->svr.state,n); - if (!waittcpsvr(tcpsvr,msg)||tcpsvr->cli[0].state!=2) return 0; + if (!waittcpsvr(tcpsvr,msg) || tcpsvr->cli[0].state!=2) return 0; - if ((nr=recv_nb(tcpsvr->cli[0].sock,buff,n))==-1) { + if ((nr=recv_nb(tcpsvr->cli[0].sock,buff,n)) == -1) { err=errsock(); tracet(1,"readtcpsvr: recv error sock=%d err=%d\n",tcpsvr->cli[0].sock,err); sprintf(msg,"recv error (%d)",err); @@ -951,14 +951,14 @@ tracet(3,"consock: sock=%d\n",tcpcli->svr.sock); /* wait re-connect */ - if (tcpcli->svr.tcon<0||(tcpcli->svr.tcon>0&& + if (tcpcli->svr.tcon<0 || (tcpcli->svr.tcon>0 && (int)(tickget()-tcpcli->svr.tdis)svr.tcon)) { return 0; } /* non-block connect */ if ((stat=connect_nb(tcpcli->svr.sock,(struct sockaddr *)&tcpcli->svr.addr, - sizeof(tcpcli->svr.addr)))==-1) + sizeof(tcpcli->svr.addr))) == -1) { err=errsock(); sprintf(msg,"connect error (%d)",err); @@ -1021,17 +1021,17 @@ if (tcpcli->svr.state<0) return 0; - if (tcpcli->svr.state==0) + if (tcpcli->svr.state == 0) { /* close */ if (!gentcp(&tcpcli->svr,1,msg)) return 0; } - if (tcpcli->svr.state==1) + if (tcpcli->svr.state == 1) { /* wait */ if (!consock(tcpcli,msg)) return 0; } - if (tcpcli->svr.state==2) + if (tcpcli->svr.state == 2) { /* connect */ - if (tcpcli->toinact>0&& + if (tcpcli->toinact>0 && (int)(tickget()-tcpcli->svr.tact)>tcpcli->toinact) { sprintf(msg,"timeout"); @@ -1053,7 +1053,7 @@ if (!waittcpcli(tcpcli,msg)) return 0; - if ((nr=recv_nb(tcpcli->svr.sock,buff,n))==-1) + if ((nr=recv_nb(tcpcli->svr.sock,buff,n)) == -1) { err=errsock(); tracet(1,"readtcpcli: recv error sock=%d err=%d\n",tcpcli->svr.sock,err); @@ -1076,7 +1076,7 @@ if (!waittcpcli(tcpcli,msg)) return 0; - if ((ns=send_nb(tcpcli->svr.sock,buff,n))==-1) + if ((ns=send_nb(tcpcli->svr.sock,buff,n)) == -1) { err=errsock(); tracet(1,"writetcp: send error sock=%d err=%d\n",tcpcli->svr.sock,err); @@ -1110,7 +1110,7 @@ { for (k=b=0;k<6;k++,i++) { - b<<=1; if (i/8>(7-i%8))&0x1; + b<<=1; if (i/8>(7-i%8))&0x1; } str[j++]=table[b]; } @@ -1128,10 +1128,10 @@ tracet(3,"reqntrip_s: state=%d\n",ntrip->state); - p+=sprintf(p,"SOURCE %s %s\r\n",ntrip->passwd,ntrip->mntpnt); - p+=sprintf(p,"Source-Agent: NTRIP %s\r\n",NTRIP_AGENT); - p+=sprintf(p,"STR: %s\r\n",ntrip->str); - p+=sprintf(p,"\r\n"); + p += sprintf(p,"SOURCE %s %s\r\n",ntrip->passwd,ntrip->mntpnt); + p += sprintf(p,"Source-Agent: NTRIP %s\r\n",NTRIP_AGENT); + p += sprintf(p,"STR: %s\r\n",ntrip->str); + p += sprintf(p,"\r\n"); if (writetcpcli(ntrip->tcp,(unsigned char *)buff,p-buff,msg)!=p-buff) return 0; @@ -1149,22 +1149,22 @@ tracet(3,"reqntrip_c: state=%d\n",ntrip->state); - p+=sprintf(p,"GET %s/%s HTTP/1.0\r\n",ntrip->url,ntrip->mntpnt); - p+=sprintf(p,"User-Agent: NTRIP %s\r\n",NTRIP_AGENT); + p += sprintf(p,"GET %s/%s HTTP/1.0\r\n",ntrip->url,ntrip->mntpnt); + p += sprintf(p,"User-Agent: NTRIP %s\r\n",NTRIP_AGENT); if (!*ntrip->user) { - p+=sprintf(p,"Accept: */*\r\n"); - p+=sprintf(p,"Connection: close\r\n"); + p += sprintf(p,"Accept: */*\r\n"); + p += sprintf(p,"Connection: close\r\n"); } else { sprintf(user,"%s:%s",ntrip->user,ntrip->passwd); - p+=sprintf(p,"Authorization: Basic "); - p+=encbase64(p,(unsigned char *)user,strlen(user)); - p+=sprintf(p,"\r\n"); + p += sprintf(p,"Authorization: Basic "); + p += encbase64(p,(unsigned char *)user,strlen(user)); + p += sprintf(p,"\r\n"); } - p+=sprintf(p,"\r\n"); + p += sprintf(p,"\r\n"); if (writetcpcli(ntrip->tcp,(unsigned char *)buff,p-buff,msg)!=p-buff) return 0; @@ -1188,9 +1188,9 @@ if ((p=strstr((char *)ntrip->buff,NTRIP_RSP_OK_SVR))) { /* ok */ q=(char *)ntrip->buff; - p+=strlen(NTRIP_RSP_OK_SVR); - ntrip->nb-=p-q; - for (i=0;inb;i++) *q++=*p++; + p += strlen(NTRIP_RSP_OK_SVR); + ntrip->nb -= p-q; + for (i=0;inb;i++) *q++ = *p++; ntrip->state=2; sprintf(msg,"%s/%s",ntrip->tcp->svr.saddr,ntrip->mntpnt); tracet(2,"rspntrip_s: response ok nb=%d\n",ntrip->nb); @@ -1233,9 +1233,9 @@ if ((p=strstr((char *)ntrip->buff,NTRIP_RSP_OK_CLI))) { /* ok */ q=(char *)ntrip->buff; - p+=strlen(NTRIP_RSP_OK_CLI); - ntrip->nb-=p-q; - for (i=0;inb;i++) *q++=*p++; + p += strlen(NTRIP_RSP_OK_CLI); + ntrip->nb -= p-q; + for (i=0;inb;i++) *q++ = *p++; ntrip->state=2; sprintf(msg,"%s/%s",ntrip->tcp->svr.saddr,ntrip->mntpnt); tracet(2,"rspntrip_c: response ok nb=%d\n",ntrip->nb); @@ -1293,25 +1293,25 @@ if (ntrip->tcp->svr.state<2) ntrip->state=0; /* tcp disconnected */ - if (ntrip->state==0) + if (ntrip->state == 0) { /* send request */ - if (!(ntrip->type==0?reqntrip_s(ntrip,msg):reqntrip_c(ntrip,msg))) + if (!(ntrip->type == 0?reqntrip_s(ntrip,msg):reqntrip_c(ntrip,msg))) { return 0; } tracet(2,"waitntrip: state=%d nb=%d\n",ntrip->state,ntrip->nb); } - if (ntrip->state==1) { /* read response */ + if (ntrip->state == 1) { /* read response */ p=(char *)ntrip->buff+ntrip->nb; - if ((n=readtcpcli(ntrip->tcp,(unsigned char *)p,NTRIP_MAXRSP-ntrip->nb-1,msg))==0) + if ((n=readtcpcli(ntrip->tcp,(unsigned char *)p,NTRIP_MAXRSP-ntrip->nb-1,msg)) == 0) { tracet(5,"waitntrip: readtcp n=%d\n",n); return 0; } - ntrip->nb+=n; ntrip->buff[ntrip->nb]='\0'; + ntrip->nb += n; ntrip->buff[ntrip->nb]='\0'; /* wait response */ - return ntrip->type==0?rspntrip_s(ntrip,msg):rspntrip_c(ntrip,msg); + return ntrip->type == 0?rspntrip_s(ntrip,msg):rspntrip_c(ntrip,msg); } return 1; } @@ -1404,7 +1404,7 @@ /* get state ntrip -----------------------------------------------------------*/ int statentrip(ntrip_t *ntrip) { - return !ntrip?0:(ntrip->state==0?ntrip->tcp->svr.state:ntrip->state); + return !ntrip?0:(ntrip->state == 0?ntrip->tcp->svr.state:ntrip->state); } @@ -1441,7 +1441,7 @@ if ((p=strrchr(buff,'@'))) { - *p++='\0'; + *p++ = '\0'; if ((q=strchr(buff,':'))) { *q='\0'; if (passwd) strcpy(passwd,q+1); @@ -1469,7 +1469,7 @@ tow=time2gpst(time,&week); /* next retry time */ - if (stat==0&&topts[3]>0) + if (stat == 0 && topts[3]>0) { tow=(floor((tow-topts[2])/topts[3])+1.0)*topts[3]+topts[2]; return gpst2time(week,tow); @@ -1512,9 +1512,9 @@ /* if local file exist, skip download */ strcpy(tmpfile,local); - if ((p=strrchr(tmpfile,'.'))&& - (!strcmp(p,".z")||!strcmp(p,".gz")||!strcmp(p,".zip")|| - !strcmp(p,".Z")||!strcmp(p,".GZ")||!strcmp(p,".ZIP"))) + if ((p=strrchr(tmpfile,'.')) && + (!strcmp(p,".z") || !strcmp(p,".gz") || !strcmp(p,".zip") || + !strcmp(p,".Z") || !strcmp(p,".GZ") || !strcmp(p,".ZIP"))) { *p='\0'; } @@ -1534,7 +1534,7 @@ proxyopt="--proxy=on "; } /* download command (ref [2]) */ - if (ftp->proto==0) + if (ftp->proto == 0) { /* ftp */ sprintf(opt,"--ftp-user=%s --ftp-password=%s --glob=off --passive-ftp %s-t 1 -T %d -O \"%s\"", ftp->user,ftp->passwd,proxyopt,FTP_TIMEOUT,local); @@ -1559,9 +1559,9 @@ remove(errfile); /* uncompress downloaded file */ - if ((p=strrchr(local,'.'))&& - (!strcmp(p,".z")||!strcmp(p,".gz")||!strcmp(p,".zip")|| - !strcmp(p,".Z")||!strcmp(p,".GZ")||!strcmp(p,".ZIP"))) + if ((p=strrchr(local,'.')) && + (!strcmp(p,".z") || !strcmp(p,".gz") || !strcmp(p,".zip") || + !strcmp(p,".Z") || !strcmp(p,".GZ") || !strcmp(p,".ZIP"))) { if (rtk_uncompress(local,tmpfile)) { @@ -1649,7 +1649,7 @@ } if (ftp->state<=1) return 0; /* ftp/http on going? */ - if (ftp->state==3) + if (ftp->state == 3) { /* ftp error */ sprintf(msg,"%s error (%d)",ftp->proto?"http":"ftp",ftp->error); @@ -1661,8 +1661,8 @@ /* return local file path if ftp completed */ p=buff; q=(unsigned char *)ftp->local; - while (*q&&(int)(p-buff)tnext=nextdltime(ftp->topts,1); @@ -1677,7 +1677,7 @@ /* get state ftp -------------------------------------------------------------*/ int stateftp(ftp_t *ftp) { - return !ftp?0:(ftp->state==0?2:(ftp->state<=2?3:-1)); + return !ftp?0:(ftp->state == 0?2:(ftp->state<=2?3:-1)); } @@ -1823,10 +1823,10 @@ void strsync(stream_t *stream1, stream_t *stream2) { file_t *file1,*file2; - if (stream1->type!=STR_FILE||stream2->type!=STR_FILE) return; + if (stream1->type!=STR_FILE || stream2->type!=STR_FILE) return; file1=(file_t*)stream1->port; file2=(file_t*)stream2->port; - if (file1&&file2) syncfile(file1,file2); + if (file1 && file2) syncfile(file1,file2); } /* lock/unlock stream ---------------------------------------------------------- @@ -1854,7 +1854,7 @@ tracet(4,"strread: n=%d\n",n); - if (!(stream->mode&STR_MODE_R)||!stream->port) return 0; + if (!(stream->mode&STR_MODE_R) || !stream->port) return 0; strlock(stream); @@ -1871,7 +1871,7 @@ strunlock(stream); return 0; } - stream->inb+=nr; + stream->inb += nr; tick=tickget(); if (nr>0) stream->tact=tick; if ((int)(tick-stream->tick)>=tirate) @@ -1900,7 +1900,7 @@ tracet(3,"strwrite: n=%d\n",n); - if (!(stream->mode&STR_MODE_W)||!stream->port) return 0; + if (!(stream->mode&STR_MODE_W) || !stream->port) return 0; strlock(stream); @@ -1918,7 +1918,7 @@ strunlock(stream); return 0; } - stream->outb+=ns; + stream->outb += ns; tick=tickget(); if (ns>0) stream->tact=tick; if ((int)(tick-stream->tick)>tirate) @@ -1967,7 +1967,7 @@ strunlock(stream); return 0; } - if (state==2&&(int)(tickget()-stream->tact)<=TINTACT) state=3; + if (state == 2 && (int)(tickget()-stream->tact)<=TINTACT) state=3; strunlock(stream); return state; } @@ -2013,7 +2013,7 @@ tracet(3,"strsetopt: opt=%d %d %d %d %d %d %d %d\n",opt[0],opt[1],opt[2], opt[3],opt[4],opt[5],opt[6],opt[7]); - toinact =0=1s */ + toinact =0=1s */ ticonnect =opt[1]<1000?1000:opt[1]; /* >=1s */ tirate =opt[2]<100 ?100 :opt[2]; /* >=0.1s */ buffsize =opt[3]<4096?4096:opt[3]; /* >=4096byte */ @@ -2034,11 +2034,11 @@ tracet(3,"strsettimeout: toinact=%d tirecon=%d\n",toinact,tirecon); - if (stream->type==STR_TCPCLI) + if (stream->type == STR_TCPCLI) { tcpcli=(tcpcli_t *)stream->port; } - else if (stream->type==STR_NTRIPCLI||stream->type==STR_NTRIPSVR) + else if (stream->type == STR_NTRIPCLI || stream->type == STR_NTRIPSVR) { tcpcli=((ntrip_t *)stream->port)->tcp; } @@ -2081,7 +2081,7 @@ gtime_t strgettime(stream_t *stream) { file_t *file; - if (stream->type==STR_FILE&&(stream->mode&STR_MODE_R)&& + if (stream->type == STR_FILE && (stream->mode&STR_MODE_R) && (file=(file_t *)stream->port)) { return timeadd(file->time,file->start); /* replay start time */ @@ -2123,13 +2123,13 @@ trace(4,"gen_hex: msg=%s\n",msg); strncpy(mbuff,msg,1023); - for (p=strtok(mbuff," ");p&&narg<256;p=strtok(NULL," ")) + for (p=strtok(mbuff," ");p && narg<256;p=strtok(NULL," ")) { args[narg++]=p; } for (i=0;i