/* 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);
}
