/* update lex ephemeris ------------------------------------------------------*/ static int updatelex(int index, gtime_t time, lex_t *lex, nav_t *nav) { gtime_t tof; for (;index<lex->n;index++) { if (!lexupdatecorr(lex->msgs+index,nav,&tof)) continue; fprintf(stderr,"%6d: tof=%s\r",index,time_str(tof,0)); if (timediff(tof,time)>=0.0) break; } return index; }
/* input obs data, navigation messages and sbas correction -------------------*/ static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt) { gtime_t time={0}; char path[1024]; int i,nu,nr,n=0; trace(3,"infunc : revs=%d iobsu=%d iobsr=%d isbs=%d\n",revs,iobsu,iobsr,isbs); if (0<=iobsu&&iobsu<obss.n) { settime((time=obss.data[iobsu].time)); if (checkbrk("processing : %s Q=%d",time_str(time,0),solq)) { aborts=1; showmsg("aborted"); return -1; } } if (!revs) { /* input forward data */ if ((nu=nextobsf(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { for (;(nr=nextobsf(&obss,&iobsr,2))>0;iobsr+=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)>-DTTOL) break; } else { for (i=iobsr;(nr=nextobsf(&obss,&i,2))>0;iobsr=i,i+=nr) if (timediff(obss.data[i].time,obss.data[iobsu].time)>DTTOL) break; } nr=nextobsf(&obss,&iobsr,2); for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu+i]; for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr+i]; iobsu+=nu; /* update sbas corrections */ while (isbs<sbss.n) { time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow); if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */ sbsupdatecorr(sbss.msgs+isbs,&navs); } if (timediff(time,obs[0].time)>-1.0-DTTOL) break; isbs++; } /* update lex corrections */ while (ilex<lexs.n) { if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) { if (timediff(time,obs[0].time)>-1.0-DTTOL) break; } ilex++; } /* update rtcm corrections */ if (*rtcm_file) { /* open or swap rtcm file */ reppath(rtcm_file,path,obs[0].time,"",""); if (strcmp(path,rtcm_path)) { strcpy(rtcm_path,path); if (fp_rtcm) fclose(fp_rtcm); fp_rtcm=fopen(path,"rb"); if (fp_rtcm) { rtcm.time=obs[0].time; input_rtcm3f(&rtcm,fp_rtcm); trace(2,"rtcm file open: %s\n",path); } } if (fp_rtcm) { while (timediff(rtcm.time,obs[0].time)<0.0) { if (input_rtcm3f(&rtcm,fp_rtcm)<-1) break; } for (i=0;i<MAXSAT;i++) navs.ssr[i]=rtcm.ssr[i]; } } } else { /* input backward data */ if ((nu=nextobsb(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { for (;(nr=nextobsb(&obss,&iobsr,2))>0;iobsr-=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)<DTTOL) break; } else { for (i=iobsr;(nr=nextobsb(&obss,&i,2))>0;iobsr=i,i-=nr) if (timediff(obss.data[i].time,obss.data[iobsu].time)<-DTTOL) break; } nr=nextobsb(&obss,&iobsr,2); for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu-nu+1+i]; for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr-nr+1+i]; iobsu-=nu; /* update sbas corrections */ while (isbs>=0) { time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow); if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */ sbsupdatecorr(sbss.msgs+isbs,&navs); } if (timediff(time,obs[0].time)<1.0+DTTOL) break; isbs--; } /* update lex corrections */ while (ilex>=0) { if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) { if (timediff(time,obs[0].time)<1.0+DTTOL) break; } ilex--; } } return n; }
/* update rtk server struct --------------------------------------------------*/ static void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat, sbsmsg_t *sbsmsg, int index, int iobs) { eph_t *eph1,*eph2,*eph3; geph_t *geph1,*geph2,*geph3; gtime_t tof; double pos[3],del[3],dr[3]; int i,n=0,prn=0,sbssat=svr->rtk.opt.sbassatsel,sys=0,iode=0; INIT_ZERO(del); INIT_ZERO(dr); INIT_ZERO(pos); INIT_ZERO(prn); tracet(4,"updatesvr: ret=%d sat=%2d index=%d\n",ret,sat,index); if (ret==1) { /* observation data */ if (iobs<MAXOBSBUF) { for (i=0;i<obs->n;i++) { 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].n=n; sortobs(&svr->obs[index][iobs]); } svr->nmsg[index][0]++; } else if (ret==2) { /* ephemeris */ if (satsys(sat,&prn)!=SYS_GLO) { 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)) { *eph3=*eph2; *eph2=*eph1; updatenav(&svr->nav); } } svr->nmsg[index][1]++; } else { 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)) { *geph3=*geph2; *geph2=*geph1; updatenav(&svr->nav); updatefcn(svr); } } svr->nmsg[index][6]++; } } else if (ret==3) { /* sbas message */ if (sbsmsg&&(sbssat==sbsmsg->prn||sbssat==0)) { if (svr->nsbs<MAXSBSMSG) { svr->sbsmsg[svr->nsbs++]=*sbsmsg; } else { for (i=0;i<MAXSBSMSG-1;i++) svr->sbsmsg[i]=svr->sbsmsg[i+1]; svr->sbsmsg[i]=*sbsmsg; } sbsupdatecorr(sbsmsg,&svr->nav); } svr->nmsg[index][3]++; } else if (ret==9) { /* ion/utc parameters */ 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; } svr->nmsg[index][2]++; } else if (ret==5) { /* antenna postion parameters */ if (svr->rtk.opt.refpos==4&&index==1) { for (i=0;i<3;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; enu2ecef(pos,del,dr); for (i=0;i<3;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++) { svr->rtk.rb[i]+=dr[i]; } } } svr->nmsg[index][4]++; } else if (ret==7) { /* dgps correction */ svr->nmsg[index][5]++; } else if (ret==10) { /* ssr message */ for (i=0;i<MAXSAT;i++) { if (!svr->rtcm[index].ssr[i].update) continue; svr->rtcm[index].ssr[i].update=0; 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 (svr->nav.eph[i ].iode!=iode&& svr->nav.eph[i+MAXSAT].iode!=iode) { continue; } } else if (sys==SYS_GLO) { 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->nmsg[index][7]++; } else if (ret==31) { /* lex message */ lexupdatecorr(&svr->raw[index].lexmsg,&svr->nav,&tof); svr->nmsg[index][8]++; } else if (ret==-1) { /* error */ svr->nmsg[index][9]++; } }
/* input obs data, navigation messages and sbas correction -------------------*/ static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt) { gtime_t time={0}; int i,nu,nr,n=0; char str[32]; trace(3,"infunc : revs=%d iobsu=%d iobsr=%d isbs=%d\n",revs,iobsu,iobsr,isbs); if (0<=iobsu&&iobsu<obss.n) { settime((time=obss.data[iobsu].time)); if (checkbrk("processing : %s Q=%d",time_str(time,0),solq)) { aborts=1; showmsg("aborted"); return -1; } } if (!revs) { /* input forward data */ if ((nu=nextobsf(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { for (;(nr=nextobsf(&obss,&iobsr,2))>0;iobsr+=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)>-DTTOL) break; } else { for (i=iobsr;(nr=nextobsf(&obss,&i,2))>0;iobsr=i,i+=nr) if (timediff(obss.data[i].time,obss.data[iobsu].time)>DTTOL) break; } nr=nextobsf(&obss,&iobsr,2); for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu+i]; for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr+i]; iobsu+=nu; /* update sbas corrections */ while (isbs<sbss.n) { time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow); if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */ sbsupdatecorr(sbss.msgs+isbs,&navs); } if (timediff(time,obs[0].time)>-1.0-DTTOL) break; isbs++; } /* update lex corrections */ while (ilex<lexs.n) { if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) { if (timediff(time,obs[0].time)>-1.0-DTTOL) break; } ilex++; } /* update rtcm corrections */ if (fp_rtcm) { if (rtcm.time.time==0) rtcm.time=obs[0].time; #if 1 while (timediff(rtcm.time,obs[0].time)<-1.0) { #else while (timediff(rtcm.time,obs[0].time)<5.0) { #endif if (input_rtcm3f(&rtcm,fp_rtcm)==-2) break; } for (i=0;i<MAXSAT;i++) navs.ssr[i]=rtcm.ssr[i]; } } else { /* input backward data */ if ((nu=nextobsb(&obss,&iobsu,1))<=0) return -1; if (popt->intpref) { for (;(nr=nextobsb(&obss,&iobsr,2))>0;iobsr-=nr) if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)<DTTOL) break; } else { for (i=iobsr;(nr=nextobsb(&obss,&i,2))>0;iobsr=i,i-=nr) if (timediff(obss.data[i].time,obss.data[iobsu].time)<-DTTOL) break; } nr=nextobsb(&obss,&iobsr,2); for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu-nu+1+i]; for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr-nr+1+i]; iobsu-=nu; /* update sbas corrections */ while (isbs>=0) { time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow); if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */ sbsupdatecorr(sbss.msgs+isbs,&navs); } if (timediff(time,obs[0].time)<1.0+DTTOL) break; isbs--; } /* update lex corrections */ while (ilex>=0) { if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) { if (timediff(time,obs[0].time)<1.0+DTTOL) break; } ilex--; } } return n; } /* process positioning -------------------------------------------------------*/ static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt, int mode) { gtime_t time={0}; sol_t sol={{0}}; rtk_t rtk; obsd_t obs[MAXOBS]; double rb[3]={0}; int i,nobs,n,solstatic,pri[]={0,1,2,3,4,5,1,6}; trace(3,"procpos : mode=%d\n",mode); solstatic=sopt->solstatic&& (popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC); rtkinit(&rtk,popt); while ((nobs=inputobs(obs,rtk.sol.stat,popt))>=0) { /* exclude satellites */ for (i=n=0;i<nobs;i++) { if ((satsys(obs[i].sat,NULL)&popt->navsys)&& popt->exsats[obs[i].sat-1]!=1) obs[n++]=obs[i]; } if (n<=0) continue; if (!rtkpos(&rtk,obs,n,&navs)) continue; if (mode==0) { /* forward/backward */ if (!solstatic) { outsol(fp,&rtk.sol,rtk.rb,sopt); } else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) { sol=rtk.sol; for (i=0;i<3;i++) rb[i]=rtk.rb[i]; if (time.time==0||timediff(rtk.sol.time,time)<0.0) { time=rtk.sol.time; } } } else if (!revs) { /* combined-forward */ if (isolf>=nepoch) return; solf[isolf]=rtk.sol; for (i=0;i<3;i++) rbf[i+isolf*3]=rtk.rb[i]; isolf++; } else { /* combined-backward */ if (isolb>=nepoch) return; solb[isolb]=rtk.sol; for (i=0;i<3;i++) rbb[i+isolb*3]=rtk.rb[i]; isolb++; } } if (mode==0&&solstatic&&time.time!=0.0) { sol.time=time; outsol(fp,&sol,rb,sopt); } rtkfree(&rtk); }