/* tle_pos() accuracy --------------------------------------------------------*/ static void utest3(void) { const char *file1="../data/tle/brdc3050.12*"; const char *file2="../data/tle/TLE_GNSS_20121101.txt"; const char *file3="../data/tle/igs17127.erp"; const double ep[6]={2012,10,31,0,0,0}; nav_t nav={0}; erp_t erp={0}; tle_t tle={0}; gtime_t time; char sat[32]; double rs1[6],rs2[6],ds[6],dts[2],var; int i,j,k,stat,svh; readrnx(file1,0,"",NULL,&nav,NULL); assert(nav.n>0); stat=readerp(file3,&erp); assert(stat); stat=tle_read(file2,&tle); assert(stat); for (i=0;i<MAXSAT;i++) { satno2id(i+1,sat); fprintf(OUT,"SAT=%s\n",sat); for (j=0;j<96;j++) { time=timeadd(epoch2time(ep),900.0*j); if (!satpos(time,time,i+1,EPHOPT_BRDC,&nav,rs1,dts,&var,&svh)) continue; if (satsys(i+1,NULL)==SYS_QZS) svh&=0xFE; if (svh) continue; stat=tle_pos(time,sat,"","",&tle,&erp,rs2); assert(stat); for (k=0;k<3;k++) ds[k]=rs2[k]-rs1[k]; fprintf(OUT,"%6.0f %11.3f %11.3f %11.3f %11.3f\n",900.0*j, ds[0]/1e3,ds[1]/1e3,ds[2]/1e3,norm(ds,3)/1e3); assert(norm(ds,3)/1e3<300.0); } fprintf(OUT,"\n"); } fprintf(OUT,"%s utest3 : OK\n",__FILE__); }
// save dop ----------------------------------------------------------------- void __fastcall TPlot::SaveDop(AnsiString file) { FILE *fp; gtime_t time; double azel[MAXOBS*2],dop[4],tow; int i,j,ns,week; char tstr[64],*tlabel; trace(3,"SaveDop: file=%s\n",file.c_str()); if (!(fp=fopen(file.c_str(),"w"))) return; tlabel=TimeLabel<=1?"TIME (GPST)":(TimeLabel<=2?"TIME (UTC)":"TIME (JST)"); fprintf(fp,"%% %-*s %6s %8s %8s %8s %8s (EL>=%.0fdeg)\n",TimeLabel==0?13:19, tlabel,"NSAT","GDOP","PDOP","HDOP","VDOP",ElMask); for (i=0;i<NObs;i++) { ns=0; for (j=IndexObs[i];j<Obs.n&&j<IndexObs[i+1];j++) { if (SatMask[Obs.data[j].sat-1]) continue; if (El[j]<ElMask*D2R) continue; if (ElMaskP&&El[j]<ElMaskData[(int)(Az[j]*R2D+0.5)]) continue; azel[ ns*2]=Az[j]; azel[1+ns*2]=El[j]; ns++; } if (ns<=0) continue; dops(ns,azel,ElMask*D2R,dop); time=Obs.data[IndexObs[i]].time; if (TimeLabel==0) { tow=time2gpst(time,&week); sprintf(tstr,"%4d %8.1f ",week,tow); } else if (TimeLabel==1) { time2str(time,tstr,1); } else if (TimeLabel==2) { time2str(gpst2utc(time),tstr,1); } else { time2str(timeadd(gpst2utc(time),9*3600.0),tstr,1); } fprintf(fp,"%s %6d %8.1f %8.1f %8.1f %8.1f\n",tstr,ns,dop[0],dop[1], dop[2], dop[3]); } fclose(fp); }
/* decode type 19: rtk uncorrected pseudorange -------------------------------*/ static int decode_type19(rtcm_t *rtcm) { gtime_t time; double usec,pr,tt; int i=48,index,freq,sync=1,code,sys,prn,sat; rtklib_trace(4,"decode_type19: len=%d\n",rtcm->len); if (i+24<=rtcm->len*8) { freq=getbitu(rtcm->buff,i, 2); i+= 2+2; usec=getbitu(rtcm->buff,i,20); i+=20; } else { rtklib_trace(2,"rtcm2 19 length error: len=%d\n",rtcm->len); return -1; } if (freq&0x1) { rtklib_trace(2,"rtcm2 19 not supported frequency: freq=%d\n",freq); return -1; } freq>>=1; while (i+48<=rtcm->len*8&&rtcm->obs.n<MAXOBS) { sync=getbitu(rtcm->buff,i, 1); i+= 1; code=getbitu(rtcm->buff,i, 1); i+= 1; sys =getbitu(rtcm->buff,i, 1); i+= 1; prn =getbitu(rtcm->buff,i, 5); i+= 5+8; pr =getbitu(rtcm->buff,i,32); i+=32; if (prn==0) prn=32; if (!(sat=satno(sys?SYS_GLO:SYS_GPS,prn))) { rtklib_trace(2,"rtcm2 19 satellite number error: sys=%d prn=%d\n",sys,prn); continue; } time=timeadd(rtcm->time,usec*1E-6); if (sys) time=utc2gpst(time); /* convert glonass time to gpst */ tt=timediff(rtcm->obs.data[0].time,time); if (rtcm->obsflag||fabs(tt)>1E-9) { rtcm->obs.n=rtcm->obsflag=0; } if ((index=obsindex(&rtcm->obs,time,sat))>=0) { rtcm->obs.data[index].P[freq]=pr*0.02; rtcm->obs.data[index].code[freq]= !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); } } rtcm->obsflag=!sync; return sync?0:1; }
// update information for current-cursor position --------------------------- void __fastcall TPlot::UpdatePoint(int x, int y) { gtime_t time; TPoint p(x,y); double enu[3]={0},rr[3],pos[3],xx,yy,r,xl[2],yl[2],q[2],az,el,snr; int i; char tstr[64]; AnsiString msg; trace(4,"UpdatePoint: x=%d y=%d\n",x,y); if (PlotType==PLOT_TRK) { // track-plot if (norm(OPos,3)>0.0) { GraphT->ToPos(p,enu[0],enu[1]); ecef2pos(OPos,pos); enu2ecef(pos,enu,rr); for (i=0;i<3;i++) rr[i]+=OPos[i]; ecef2pos(rr,pos); msg=LatLonStr(pos,8); } } else if (PlotType==PLOT_SKY||PlotType==PLOT_MPS) { // sky-plot GraphS->GetLim(xl,yl); GraphS->ToPos(p,q[0],q[1]); r=(xl[1]-xl[0]<yl[1]-yl[0]?xl[1]-xl[0]:yl[1]-yl[0])*0.45; if ((el=90.0-90.0*norm(q,2)/r)>0.0) { az=el>=90.0?0.0:ATAN2(q[0],q[1])*R2D; if (az<0.0) az+=360.0; msg.sprintf("AZ=%5.1f" CHARDEG " EL=%4.1f" CHARDEG,az,el); } } else if (PlotType==PLOT_SNRE) { // snr-el-plot GraphE[0]->ToPos(p,q[0],q[1]); msg.sprintf("EL=%4.1f " CHARDEG,q[0]); } else { GraphG[0]->ToPos(p,xx,yy); time=gpst2time(Week,xx); if (TimeLabel==2) time=utc2gpst(time); // UTC else if (TimeLabel==3) time=timeadd(gpst2utc(time),-9*3600.0); // JST TimeStr(time,0,1,tstr); msg=tstr; } Panel22->Visible=true; Message2->Caption=A2U(msg); }
// get position regarding time ---------------------------------------------- double __fastcall TPlot::TimePos(gtime_t time) { double tow; int week; if (TimeLabel<=1) { // www/ssss or gpst tow=time2gpst(time,&week); } else if (TimeLabel==2) { // utc tow=time2gpst(gpst2utc(time),&week); } else { // jst tow=time2gpst(timeadd(gpst2utc(time),9*3600.0),&week); } return tow+(week-Week)*86400.0*7; }
/* output reference ----------------------------------------------------------*/ static int out_ref(FILE *ofp, gtime_t ts) { double ep[6]; char file[1024]; int i; fprintf(ofp," <TR>\n"); fprintf(ofp," <TD><FONT size=\"-1\"><B>Report</B></FONT></TD>\n"); for (i=0; i<24; i++) { time2epoch(timeadd(ts,i*3600.0),ep); sprintf(file,REP_FILE,ep[0],ep[1],ep[2],ep[3]); fprintf(ofp," <TD align=\"right\"><FONT size=\"-1\"><A href=\"../%s\">%02.0fh</A></FONT></TD>\n", file,ep[3]); } fprintf(ofp," </TR>\n"); return 1; }
/* satellite position and clock by broadcast ephemeris -----------------------*/ static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav, int iode, double *rs, double *dts, double *var, int *svh,char **msg) { eph_t *eph; double rst[3],dtst[1],tt=1E-3; int i; *svh=-1; if (!(eph=seleph(teph,sat,iode,nav,msg))) return 0; //start=TIM2->CNT; eph2pos(time,eph,rs,dts,var); //SendIntStr(TIM2->CNT-start); time=timeadd(time,tt); eph2pos(time,eph,rst,dtst,var); *svh=eph->svh; /* satellite velocity and clock drift by differential approx */ for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt; dts[1]=(dtst[0]-dts[0])/tt; return 1; }
/* satellite position/clock by precise ephemeris/clock ------------------------- * compute satellite position/clock with precise ephemeris/clock * args : gtime_t time I time (gpst) * int sat I satellite number * nav_t *nav I navigation data * int opt I sat postion option * (0: center of mass, 1: antenna phase center) * double *rs O sat position and velocity (ecef) * {x,y,z,vx,vy,vz} (m|m/s) * double *dts O sat clock {bias,drift} (s|s/s) * double *var IO sat position and clock error variance (m) * (NULL: no output) * return : status (1:ok,0:error or data outage) * notes : clock includes relativistic correction but does not contain code bias * before calling the function, nav->peph, nav->ne, nav->pclk and * nav->nc must be set by calling readsp3(), readrnx() or readrnxt() * if precise clocks are not set, clocks in sp3 are used instead *-----------------------------------------------------------------------------*/ extern int peph2pos(gtime_t time, int sat, const nav_t *nav, int opt, double *rs, double *dts, double *var) { double rss[3],rst[3],dtss[1],dtst[1],dant[3]={0},vare=0.0,varc=0.0,tt=1E-3; int i; trace(4,"peph2pos: time=%s sat=%2d opt=%d\n",time_str(time,3),sat,opt); if (sat<=0||MAXSAT<sat) return 0; /* satellite position and clock bias */ if (!pephpos(time,sat,nav,rss,dtss,&vare,&varc)|| !pephclk(time,sat,nav,dtss,&varc)) return 0; time=timeadd(time,tt); if (!pephpos(time,sat,nav,rst,dtst,NULL,NULL)|| !pephclk(time,sat,nav,dtst,NULL)) return 0; /* satellite antenna offset correction */ if (opt) { satantoff(time,rss,nav->pcvs+sat-1,dant); } for (i=0;i<3;i++) { rs[i ]=rss[i]+dant[i]; rs[i+3]=(rst[i]-rss[i])/tt; } /* relativistic effect correction */ if (dtss[0]!=0.0) { dts[0]=dtss[0]-2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT; dts[1]=(dtst[0]-dtss[0])/tt; } else { /* no precise clock */ dts[0]=dts[1]=0.0; } if (var) *var=vare+varc; return 1; }
/* decode raw ephemereris ----------------------------------------------------*/ static int decode_gw10reph(raw_t *raw) { eph_t eph={0}; double tow; int i,week,prn,sat; unsigned char *p=raw->buff+2,buff[90]; trace(4,"decode_gw10reph: len=%d\n",raw->len); prn=U1(p); if (!(sat=satno(SYS_GPS,prn))) { trace(2,"gw10 raw ephemeris satellite number error: prn=%d\n",prn); return -1; } for (i=0;i<90;i++) { buff[i]=*(p+1+i); } if (decode_frame(buff ,&eph,NULL,NULL,NULL,NULL)!=1|| decode_frame(buff+ 30,&eph,NULL,NULL,NULL,NULL)!=2|| decode_frame(buff+ 60,&eph,NULL,NULL,NULL,NULL)!=3) { trace(2,"gw10 raw ephemeris navigation frame error: prn=%d\n",prn); return -1; } /* set time if no time avaliable */ if (raw->time.time==0) { tow=getbitu(buff,24,17)*6.0; week=getbitu(buff,48,10)+OFFWEEK; raw->time=timeadd(gpst2time(week,tow),24.0); } if (!strstr(raw->opt,"-EPHALL")) { if (eph.iode==raw->nav.eph[sat-1].iode) return 0; /* unchanged */ } eph.sat=sat; raw->nav.eph[sat-1]=eph; raw->ephsat=sat; return 2; }
/* convert nav message -------------------------------------------------------*/ static void convnav(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n) { gtime_t ts1,te1,ts2,te2; int sys,prn; trace(3,"convnav :\n"); ts1=opt->ts; if (ts1.time!=0) ts1=timeadd(ts1,-MAXDTOE); te1=opt->te; if (te1.time!=0) te1=timeadd(te1, MAXDTOE); ts2=opt->ts; if (ts2.time!=0) ts2=timeadd(ts2,-MAXDTOE_GLO); te2=opt->te; if (te2.time!=0) te2=timeadd(te2, MAXDTOE_GLO); sys=satsys(str->sat,&prn)&opt->navsys; if (sys==SYS_GPS) { if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return; if (ofp[1]) { /* output rinex nav */ outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1); n[1]++; } } else if (sys==SYS_GLO) { if (opt->exsats[str->sat-1]==1||!screent(str->time,ts2,te2,0.0)) return; if (ofp[1]&&opt->rnxver>2.99) { /* output rinex nav */ outrnxgnavb(ofp[1],opt,str->nav->geph+prn-1); n[1]++; } if (ofp[2]&&opt->rnxver<=2.99) { /* output rinex gnav */ outrnxgnavb(ofp[2],opt,str->nav->geph+prn-1); n[2]++; } } else if (sys==SYS_SBS) { if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return; if (ofp[1]&&opt->rnxver>2.99) { /* output rinex nav */ outrnxhnavb(ofp[1],opt,str->nav->seph+prn-MINPRNSBS); n[1]++; } if (ofp[3]&&opt->rnxver<=2.99) { /* output rinex hnav */ outrnxhnavb(ofp[3],opt,str->nav->seph+prn-MINPRNSBS); n[3]++; } } else if (sys==SYS_QZS) { if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return; if (ofp[1]&&opt->rnxver>2.99) { /* output rinex nav */ outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1); n[1]++; } if (ofp[4]&&opt->rnxver<=2.99) { /* output rinex qnav */ outrnxnavb(ofp[4],opt,str->nav->eph+str->sat-1); n[4]++; } } else if (sys==SYS_GAL) { if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return; if (ofp[1]&&opt->rnxver>2.99) { /* output rinex nav */ outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1); n[1]++; } if (ofp[5]&&opt->rnxver<=2.99) { /* output rinex lnav */ outrnxnavb(ofp[5],opt,str->nav->eph+str->sat-1); n[5]++; } } else if (sys==SYS_CMP) { if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return; if (ofp[1]&&opt->rnxver>2.99) { /* output rinex nav */ outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1); n[1]++; } } }
/* estimate receiver position ------------------------------------------------*/ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, double *resp, char *msg) { double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig; int i,j,k,info,stat,nv,ns; trace(3,"estpos : n=%d\n",n); v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); for (i=0;i<3;i++) x[i]=sol->rr[i]; for (i=0;i<MAXITR;i++) { /* pseudorange residuals */ nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp, &ns); if (nv<NX) { sprintf(msg,"lack of valid sats ns=%d",nv); break; } /* weight by variance */ for (j=0;j<nv;j++) { sig=sqrt(var[j]); v[j]/=sig; for (k=0;k<NX;k++) H[k+j*NX]/=sig; } /* least square estimation */ if ((info=lsq(H,v,NX,nv,dx,Q))) { sprintf(msg,"lsq error info=%d",info); break; } for (j=0;j<NX;j++) x[j]+=dx[j]; if (norm(dx,NX)<1E-4) { sol->type=0; sol->time=timeadd(obs[0].time,-x[3]/CLIGHT); sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */ sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */ sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */ sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */ for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0; for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX]; sol->qr[3]=(float)Q[1]; /* cov xy */ sol->qr[4]=(float)Q[2+NX]; /* cov yz */ sol->qr[5]=(float)Q[2]; /* cov zx */ sol->ns=(unsigned char)ns; sol->age=sol->ratio=0.0; /* validate solution */ if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) { sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE; } free(v); free(H); free(var); return stat; } } if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i); free(v); free(H); free(var); return 0; }
/* rinex converter for single-session ----------------------------------------*/ static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file, char **ofile) { FILE *ofp[NOUTFILE]={NULL}; strfile_t *str; gtime_t ts={0},te={0},tend={0},time={0}; unsigned char slips[MAXSAT][NFREQ+NEXOBS]={{0}}; int i,j,nf,type,n[NOUTFILE+1]={0},abort=0; char path[1024],*paths[NOUTFILE],s[NOUTFILE][1024]; char *epath[MAXEXFILE]={0},*staid=*opt->staid?opt->staid:"0000"; trace(3,"convrnx_s: sess=%d format=%d file=%s ofile=%s %s %s %s %s %s %s\n", sess,format,file,ofile[0],ofile[1],ofile[2],ofile[3],ofile[4], ofile[5],ofile[6]); /* replace keywords in input file */ if (reppath(file,path,opt->ts,staid,"")<0) { showmsg("no time for input file: %s",file); return 0; } /* expand wild-cards in input file */ for (i=0;i<MAXEXFILE;i++) { if (!(epath[i]=(char *)malloc(1024))) { for (i=0;i<MAXEXFILE;i++) free(epath[i]); return 0; } } nf=expath(path,epath,MAXEXFILE); if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) time=opt->trtcm; if (opt->scanobs) { /* scan observation types */ if (!scan_obstype(format,epath[0],opt,&time)) return 0; } else { /* set observation types by format */ set_obstype(format,opt); } if (!(str=gen_strfile(format,opt->rcvopt,time))) { for (i=0;i<MAXEXFILE;i++) free(epath[i]); return 0; } time=opt->ts.time?opt->ts:(time.time?timeadd(time,TSTARTMARGIN):time); /* replace keywords in output file */ for (i=0;i<NOUTFILE;i++) { paths[i]=s[i]; if (reppath(ofile[i],paths[i],time,staid,"")<0) { showmsg("no time for output path: %s",ofile[i]); for (i=0;i<MAXEXFILE;i++) free(epath[i]); free_strfile(str); return 0; } } /* open output files */ if (!openfile(ofp,paths,path,opt,str->nav)) { for (i=0;i<MAXEXFILE;i++) free(epath[i]); free_strfile(str); return 0; } for (i=0;i<nf&&!abort;i++) { /* open stream file */ if (!open_strfile(str,epath[i])) continue; /* input message */ for (j=0;(type=input_strfile(str))>=-1;j++) { if (j%11==1&&(abort=showstat(sess,te,te,n))) break; /* avioid duplicated if overlapped data */ if (tend.time&&timediff(str->time,tend)<=0.0) continue; /* convert message */ switch (type) { case 1: convobs(ofp,opt,str,n,slips); break; case 2: convnav(ofp,opt,str,n); break; case 3: convsbs(ofp,opt,str,n); break; case 31: convlex(ofp,opt,str,n); break; case -1: n[NOUTFILE]++; break; /* error */ } te=str->time; if (ts.time==0) ts=te; /* set approx position */ if (type==1&&!opt->autopos&&norm(opt->apppos,3)<=0.0) { setapppos(str,opt); } if (opt->te.time&&timediff(te,opt->te)>10.0) break; } /* close stream file */ close_strfile(str); tend=te; /* end time of a file */ } /* set receiver and antenna information to option */ if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) { rtcm2opt(&str->rtcm,opt); } else if (format==STRFMT_RINEX) { rnx2opt(&str->rnx,opt); } /* close output files */ closefile(ofp,opt,str->nav); /* remove empty output files */ for (i=0;i<NOUTFILE;i++) { if (ofp[i]&&n[i]<=0) remove(ofile[i]); } if (ts.time>0) showstat(sess,ts,te,n); for (i=0;i<MAXEXFILE;i++) free(epath[i]); free_strfile(str); if (opt->tstart.time==0) opt->tstart=opt->ts; if (opt->tend .time==0) opt->tend =opt->te; return abort?-1:1; }
int main() { struct timeval now,next,diff,delay; int success; printf("%s: Starting dmx deamon\n", ProgName); // intialize USB device success = initUSB(); if ( !success ) { printf ( "%s: Error initializing USB interface\n" , ProgName ); (*exitAddr)++; return ( -1 ); } // initialize shared memory segment success = initSHM(); if ( !success ) { printf ( "%s: Error initializing shared memory\n" , ProgName ); (*exitAddr)++; return ( -2 ); } // start timer delay.tv_sec = 0; delay.tv_usec= UpdateInt; gettimeofday ( &next , NULL ); printf("%s: Init done. Entering loop cycle...\n", ProgName); // loop until commanded to shutdown while( !*exitAddr ) { // send DMX data success = sendDMX(); if ( !success ) { printf ( "%s: DMX send error\n" , ProgName ); (*exitAddr)++; } // wait for update interval timeadd ( &next , &next , &delay ); gettimeofday ( &now , NULL ); timediff ( &diff, &next , &now ); while (diff.tv_sec || diff.tv_usec) { select ( 0, NULL, NULL, NULL, &diff ); gettimeofday ( &now, NULL ); timediff ( &diff, &next, &now ); }; } printf ( "%s: dmx deamon is shutting down\n" , ProgName ); // on shutdown reset all DMX channels memset ( chanData , 0, 512 * sizeof (ubyte) ); sendDMX(); // exit the system exitUSB(); exitSHM(); return ( 0 ); }
static void *rtksvrthread(void *arg) #endif { 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],cycle,cputime; INIT_ZERO(fobs); tracet(3,"rtksvrthread:\n"); svr->state=1; obs.data=data; svr->tick=tickget(); ticknmea=svr->tick-1000; for (cycle=0;svr->state;cycle++) { tick=tickget(); for (i=0;i<3;i++) { 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) { continue; } /* write receiver raw/rtcm data to log stream */ strwrite(svr->stream+i+5,p,n); svr->nb[i]+=n; /* save peek buffer */ rtksvrlock(svr); n=n<svr->buffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i]; memcpy(svr->pbuf[i]+svr->npb[i],p,n); svr->npb[i]+=n; rtksvrunlock(svr); } for (i=0;i<3;i++) { if (svr->format[i]==STRFMT_SP3||svr->format[i]==STRFMT_RNXCLK) { /* decode download file */ decodefile(svr,i); } else { /* decode receiver raw/rtcm data */ fobs[i]=decoderaw(svr,i); } } for (i=0;i<fobs[0];i++) { /* for each rover observation data */ obs.n=0; for (j=0;j<svr->obs[0][i].n&&obs.n<MAXOBS*2;j++) { obs.data[obs.n++]=svr->obs[0][i].data[j]; } for (j=0;j<svr->obs[1][0].n&&obs.n<MAXOBS*2;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) { /* adjust current time */ 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) { 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) { writesol(svr,0); } /* send nmea request to base/nrtk input stream */ if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle) { if (svr->stream[1].state==1) { if (svr->nmeareq==1) { strsendnmea(svr->stream+1,svr->nmeapos); } else if (svr->nmeareq==2&&norm(svr->rtk.sol.rr,3)>0.0) { strsendnmea(svr->stream+1,svr->rtk.sol.rr); } } ticknmea=tick; } if ((cputime=(int)(tickget()-tick))>0) svr->cputime=cputime; /* sleep until next cycle */ sleepms(svr->cycle-cputime); } for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+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; free_raw (svr->raw +i); free_rtcm(svr->rtcm+i); } for (i=0;i<2;i++) { svr->nsb[i]=0; free(svr->sbuf[i]); svr->sbuf[i]=NULL; } return 0; }
void stop_worker(int which) { workers[which].status = DOOMED; workers[which].expiration = timeadd(getnow(),DOOM_DELAY); kill(workers[which].pid,SIGTERM); reap_workers(); }
/* combine forward/backward solutions and output results ---------------------*/ static void combres(FILE *fp, const prcopt_t *popt, const solopt_t *sopt) { gtime_t time={0}; sol_t sols={{0}},sol={{0}}; double tt,Qf[9],Qb[9],Qs[9],rbs[3]={0},rb[3]={0}; int i,j,k,solstatic,pri[]={0,1,2,3,4,5,1,6}; trace(3,"combres : isolf=%d isolb=%d\n",isolf,isolb); solstatic=sopt->solstatic&& (popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC); for (i=0,j=isolb-1;i<isolf&&j>=0;i++,j--) { if ((tt=timediff(solf[i].time,solb[j].time))<-DTTOL) { sols=solf[i]; for (k=0;k<3;k++) rbs[k]=rbf[k+i*3]; j++; } else if (tt>DTTOL) { sols=solb[j]; for (k=0;k<3;k++) rbs[k]=rbb[k+j*3]; i--; } else if (solf[i].stat<solb[j].stat) { sols=solf[i]; for (k=0;k<3;k++) rbs[k]=rbf[k+i*3]; } else if (solf[i].stat>solb[j].stat) { sols=solb[j]; for (k=0;k<3;k++) rbs[k]=rbb[k+j*3]; } else { sols=solf[i]; sols.time=timeadd(sols.time,-tt/2.0); if ((popt->mode==PMODE_KINEMA||popt->mode==PMODE_MOVEB)&& sols.stat==SOLQ_FIX) { /* degrade fix to float if validation failed */ if (!valcomb(solf+i,solb+j)) sols.stat=SOLQ_FLOAT; } for (k=0;k<3;k++) { Qf[k+k*3]=solf[i].qr[k]; Qb[k+k*3]=solb[j].qr[k]; } Qf[1]=Qf[3]=solf[i].qr[3]; Qf[5]=Qf[7]=solf[i].qr[4]; Qf[2]=Qf[6]=solf[i].qr[5]; Qb[1]=Qb[3]=solb[j].qr[3]; Qb[5]=Qb[7]=solb[j].qr[4]; Qb[2]=Qb[6]=solb[j].qr[5]; if (smoother(solf[i].rr,Qf,solb[j].rr,Qb,3,sols.rr,Qs)) continue; sols.qr[0]=(float)Qs[0]; sols.qr[1]=(float)Qs[4]; sols.qr[2]=(float)Qs[8]; sols.qr[3]=(float)Qs[1]; sols.qr[4]=(float)Qs[5]; sols.qr[5]=(float)Qs[2]; } if (!solstatic) { outsol(fp,&sols,rbs,sopt); } else if (time.time==0||pri[sols.stat]<=pri[sol.stat]) { sol=sols; for (k=0;k<3;k++) rb[k]=rbs[k]; if (time.time==0||timediff(sols.time,time)<0.0) { time=sols.time; } } } if (solstatic&&time.time!=0.0) { sol.time=time; outsol(fp,&sol,rb,sopt); } }
int main(int argc, char* argv[]) { struct timespec event_ts; // local event timestamp int num_thrds = 0; // number of user threads to simulate thread_data_t *tarr = NULL; // array of thread data structures int sel_sid = -1; // index of thread selected by scheduler thread_data_t *tsel = NULL; // pointer to the selected thread int tmp = 0; int (*sched_func)() = NULL; // scheduling function picked at run time if (argc < 3) { fprintf(stderr, usage, argv[0]); exit(1); } tmp = strlen(argv[1]); if (argv[1][tmp-1] == '+') { unblock_signal = 1; argv[1][tmp-1] = '\0'; } for (int i=0 ; i < NUM_SCHED ; i++) { if (strcmp(argv[1], MY_SCHED[i].name) == 0) { sched_func = MY_SCHED[i].func; break; } } if (sched_func == NULL) { errx(1, "Scheduling function %s not found!", argv[1]); } num_thrds = strtol(argv[2], NULL, 0); if (argc > 3) min_time = strtol(argv[3], NULL, 0); if (argc > 4) max_time = strtol(argv[4], NULL, 0); // Get simulation start time for logging reference clock_gettime(CLOCK_REALTIME, &sim_ts); pthread_mutex_init(&simlock, NULL); pthread_cond_init(&sched_cv, NULL); pthread_cond_init(&run_cv, NULL); init_thread_Q(&unblockQ, num_thrds); init_thread_Q(&readyQ, num_thrds); init_thread_Q(&blockedQ, num_thrds); pthread_mutex_lock(&simlock); // Create and fork number of threads as requested tarr = calloc(num_thrds, sizeof(thread_data_t)); for (int i=0 ; i < num_thrds ; i++) { tarr[i].sid = i; // Thread is put to blocked state right after start tarr[i].state = BLOCKED; add_to_Q_back(&blockedQ, &tarr[i]); fork_thread(&tarr[i]); LOG("Thread tid=%d created with sid=%d\n", tarr[i].tid, tarr[i].sid); } run_sid = -1; // no thread is initially running block_sid = -1; // no thread is initially entering block state do { if (block_sid != -1) { // Scheduler was waken up by the running thread blocking // Handle thread entering blocked state from running state if (run_sid != -1) errx(1, "thread[%d] was blocked while thread[%d] is still running", block_sid, run_sid); add_to_Q_back(&blockedQ, &tarr[block_sid]); block_sid = -1; } else if (run_sid != -1) { // Scheduler was waken up by the tick_ts timeout // Preempt running (and non-blocking) thread to ready state thread_data_t *t = &tarr[run_sid]; if (t->state != RUNNING) errx(1, "thread[%d] to be preempted is in %d state", run_sid, t->state); clock_gettime(CLOCK_REALTIME, &event_ts); t->remain_usec -= TIMEUSEC(&t->event_ts, &event_ts); t->event_ts = event_ts; LOG("%8ld: scheduler preempts running thread[%d] with remain_usec=%d\n", TIMEUSEC(&sim_ts, &event_ts), run_sid, t->remain_usec); // Change the preempted thread into ready state t->state = READY; add_to_Q_back(&readyQ, t); run_sid = -1; // Signal the thread to stop running pthread_cond_signal(&run_cv); } while (unblockQ.head >= 0) { // Handle threads unblocking from blocked state into ready state thread_data_t *t = remove_from_Q_front(&unblockQ); clock_gettime(CLOCK_REALTIME, &event_ts); t->event_ts = event_ts; LOG("%8ld: scheduler changes thread[%d] with remain_usec=%d to ready\n", TIMEUSEC(&sim_ts, &event_ts), t->sid, t->remain_usec); // Change the unblocking thread into ready state t->state = READY; remove_from_Q(&blockedQ, t->sid); add_to_Q_back(&readyQ, t); // Signal the thread to become formally unblocked pthread_cond_signal(&t->block_cv); } if (unblockQ.head >= 0) // unblockQ is not empty errx(1, "unblockQ has pending threads from %d to %d!?!", unblockQ.head, unblockQ.tail); if (run_sid != -1) errx(1, "scheduler is running with running thread[%d]!?!", run_sid); if (block_sid != -1) errx(1, "thread[%d] was blocking but not handled!?!", block_sid); if (readyQ.head == -1 && blockedQ.head == -1) { // no thread is running, neither in ready or blocked list clock_gettime(CLOCK_REALTIME, &event_ts); LOG("%8ld: all threads appear to have terminated\n", TIMEUSEC(&sim_ts, &event_ts)); break; } if (tsel != NULL && tsel->state == RUNNING) { // Scheduler was waken up (by unblocking or terminating threads) before // last selected thread could startrunning -- do not select again /*** Do nothing ***/ } else { // Either scheduler did not select a thread last time, or the selected // thread is not running any more (blocked, preempted or finished) // Run the scheduling algorithm to select a new thread sel_sid = sched_func(); clock_gettime(CLOCK_REALTIME, &event_ts); if (sel_sid < 0) { tsel = NULL; LOG("%8ld: scheduler cannot find any thread to run\n", TIMEUSEC(&sim_ts, &event_ts)); } else { tsel = &tarr[sel_sid]; if (tsel->state != READY) errx(1, "scheduler selects thread[%d] not in ready state", sel_sid); LOG("%8ld: scheduler selects thread[%d] with remain_usec=%d to run\n", TIMEUSEC(&sim_ts, &event_ts), sel_sid, tsel->remain_usec); // Change the selected thread to running state tsel->state = RUNNING; remove_from_Q(&readyQ, tsel->sid); // Signal the selected thread to run pthread_cond_signal(&tsel->ready_cv); } } // Scheduler wait for either a signal or tick_ts until next preemption timeadd(&event_ts, &event_ts, &tick_ts); pthread_cond_timedwait(&sched_cv, &simlock, &event_ts); } while (1); pthread_mutex_unlock(&simlock); sched_yield(); for (int i=0 ; i < num_thrds ; i++) { int rv; int tid = wait(&rv); // wait for an (only) child to exit LOG("Thread tid=%d exited with status %d\n", tid, WEXITSTATUS(rv)); } for (int i=0 ; i < num_thrds ; i++) { clear_thread(&tarr[i]); } free(tarr); destroy_thread_Q(&unblockQ); pthread_mutex_destroy(&simlock); pthread_cond_destroy(&sched_cv); pthread_cond_destroy(&run_cv); return 0; }
/* execute local file test ----------------------------------------------------- * execute local file test * args : gtime_t ts,te I time start and end * double tint I time interval (s) * url_t *urls I download urls * int nurl I number of urls * char **stas I stations * int nsta I number of stations * char *dir I local directory * int ncol I number of column * int datefmt I date format (0:year-dow,1:year-dd/mm,2:week) * FILE *fp IO log test result file pointer * return : status (1:ok,0:error,-1:aborted) *-----------------------------------------------------------------------------*/ extern void dl_test(gtime_t ts, gtime_t te, double ti, const url_t *urls, int nurl, char **stas, int nsta, const char *dir, int ncol, int datefmt, FILE *fp) { gtime_t time; double tow; char year[32],date[32],date_p[32]; int i,j,n,m,*nc,*nt,week,flag,abort=0; if (ncol<1) ncol=1; else if (ncol>200) ncol=200; fprintf(fp,"** LOCAL DATA AVAILABILITY (%s, %s) **\n\n", time_str(timeget(),0),*dir?dir:"*"); for (i=n=0;i<nurl;i++) { n+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1; } nc=imat(n,1); nt=imat(n,1); for (i=0;i<n;i++) nc[i]=nt[i]=0; for (;timediff(ts,te)<1E-3&&!abort;ts=timeadd(ts,ti*ncol)) { genpath(datefmt==0?" %Y-":"%Y/%m/","",ts,0,year); if (datefmt<=1) fprintf(fp,"%s %s",datefmt==0?"DOY ":"DATE",year); else fprintf(fp,"WEEK "); *date_p='\0'; flag=0; m=datefmt==2?1:2; for (i=0;i<(ncol+m-1)/m;i++) { time=timeadd(ts,ti*i*m); if (timediff(time,te)>=1E-3) break; if (datefmt<=1) { genpath(datefmt==0?"%n":"%d","",time,0,date); fprintf(fp,"%-4s",strcmp(date,date_p)?date:""); } else { if (fabs(time2gpst(time,&week))<1.0) { fprintf(fp,"%04d",week); flag=1; } else { fprintf(fp,"%s",flag?"":" "); flag=0; } } strcpy(date_p,date); } fprintf(fp,"\n"); for (i=j=0;i<nurl&&!abort;i++) { time=timeadd(ts,ti*ncol-1.0); if (timediff(time,te)>=0.0) time=te; /* test local files */ abort=test_locals(ts,time,ti,urls+i,stas,nsta,dir,nc+j,nt+j,fp); j+=strstr(urls[i].path,"%s")||strstr(urls[i].path,"%S")?nsta:1; } fprintf(fp,"\n"); } fprintf(fp,"# COUNT : FILES/TOTAL\n"); for (i=j=0;i<nurl;i++) { j+=print_total(urls+i,stas,nsta,nc+j,nt+j,fp); } free(nc); free(nt); }
int main(int argc,char **argv) { ros::init(argc, argv, "rtk_robot"); ROS_INFO("RTKlib for ROS Robot Edition"); ros::NodeHandle nn; ros::NodeHandle pn("~"); ros::Subscriber ecef_sub; if(pn.getParam("base_position/x", ecef_base_station.position.x) && pn.getParam("base_position/y", ecef_base_station.position.y) && pn.getParam("base_position/z", ecef_base_station.position.z)) { ROS_INFO("RTK -- Loading base station parameters from the parameter server..."); XmlRpc::XmlRpcValue position_covariance; if( pn.getParam("base_position/covariance", position_covariance) ) { ROS_ASSERT(position_covariance.getType() == XmlRpc::XmlRpcValue::TypeArray); if(position_covariance.size() != 9) { ROS_WARN("RTK -- The base station covariances are not complete! Using default values..."); } else { for(int i=0 ; i<position_covariance.size() ; ++i) { ROS_ASSERT(position_covariance[i].getType() == XmlRpc::XmlRpcValue::TypeDouble); ecef_base_station.position_covariance[i] = static_cast<double>(position_covariance[i]); } } } } else { ROS_INFO("RTK -- Subscribing to the base station for online parameters..."); ecef_sub = nn.subscribe("base_station/gps/ecef", 50, ecefCallback); } double rate; pn.param("rate", rate, 2.0); std::string gps_frame_id; pn.param<std::string>("gps_frame_id", gps_frame_id, "gps"); std::string port; pn.param<std::string>("port", port, "ttyACM0"); int baudrate; pn.param("baudrate", baudrate, 115200); ros::Publisher gps_pub = nn.advertise<sensor_msgs::NavSatFix>("gps/fix", 50); ros::Publisher status_pub = nn.advertise<rtk_msgs::Status>("gps/status", 50); ros::Subscriber gps_sub = nn.subscribe("base_station/gps/raw_data", 50, baseStationCallback); int n; //********************* rtklib stuff ********************* rtksvrinit(&server); if(server.state) { ROS_FATAL("RTK -- Failed to initialize rtklib server!"); ROS_BREAK(); } gtime_t time, time0 = {0}; int format[] = {STRFMT_UBX, STRFMT_UBX, STRFMT_RTCM2}; prcopt_t options = prcopt_default; options.mode = 2; options.nf = 1; options.navsys = SYS_GPS | SYS_SBS; options.modear = 3; options.glomodear = 0; options.minfix = 3; options.ionoopt = IONOOPT_BRDC; options.tropopt = TROPOPT_SAAS; options.rb[0] = ecef_base_station.position.x; options.rb[1] = ecef_base_station.position.y; options.rb[2] = ecef_base_station.position.z; strinitcom(); server.cycle = 10; server.nmeacycle = 1000; server.nmeareq = 0; for(int i=0 ; i<3 ; i++) server.nmeapos[i] = 0; server.buffsize = BUFFSIZE; for(int i=0 ; i<3 ; i++) server.format[i] = format[i]; server.navsel = 0; server.nsbs = 0; server.nsol = 0; server.prcout = 0; rtkfree(&server.rtk); rtkinit(&server.rtk, &options); for(int i=0 ; i<3 ; i++) { server.nb[i] = server.npb[i] = 0; if(!(server.buff[i]=(unsigned char *)malloc(BUFFSIZE)) || !(server.pbuf[i]=(unsigned char *)malloc(BUFFSIZE))) { ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!"); ROS_BREAK(); } for(int j=0 ; j<10 ; j++) server.nmsg[i][j] = 0; for(int j=0 ; j<MAXOBSBUF ; j++) server.obs[i][j].n = 0; /* initialize receiver raw and rtcm control */ init_raw(server.raw + i); init_rtcm(server.rtcm + i); /* set receiver option */ strcpy(server.raw[i].opt, ""); strcpy(server.rtcm[i].opt, ""); /* connect dgps corrections */ server.rtcm[i].dgps = server.nav.dgps; } /* output peek buffer */ for(int i=0 ; i<2 ; i++) { if (!(server.sbuf[i]=(unsigned char *)malloc(BUFFSIZE))) { ROS_FATAL("RTK -- Failed to initialize rtklib server - malloc error!"); ROS_BREAK(); } } /* set solution options */ solopt_t sol_options[2]; sol_options[0] = solopt_default; sol_options[1] = solopt_default; for(int i=0 ; i<2 ; i++) server.solopt[i] = sol_options[i]; /* set base station position */ for(int i=0 ; i<6 ; i++) server.rtk.rb[i] = i < 3 ? options.rb[i] : 0.0; /* update navigation data */ for(int i=0 ; i<MAXSAT*2 ; i++) server.nav.eph[i].ttr = time0; for(int i=0 ; i<NSATGLO*2 ; i++) server.nav.geph[i].tof = time0; for(int i=0 ; i<NSATSBS*2 ; i++) server.nav.seph[i].tof = time0; updatenav(&server.nav); /* set monitor stream */ server.moni = NULL; /* open input streams */ int stream_type[8] = {STR_SERIAL, 0, 0, 0, 0, 0, 0, 0}; char gps_path[64]; sprintf(gps_path, "%s:%d:8:n:1:off", port.c_str(), baudrate); char * paths[] = {gps_path, "localhost:27015", "", "", "", "", "", ""}; char * cmds[] = {"", "", ""}; int rw; for(int i=0 ; i<8 ; i++) { rw = i < 3 ? STR_MODE_R : STR_MODE_W; if(stream_type[i] != STR_FILE) rw |= STR_MODE_W; if(!stropen(server.stream+i, stream_type[i], rw, paths[i])) { ROS_ERROR("RTK -- Failed to open stream %s", paths[i]); for(i-- ; i>=0 ; i--) strclose(server.stream+i); ROS_FATAL("RTK -- Failed to initialize rtklib server - failed to open all streams!"); ROS_BREAK(); } /* set initial time for rtcm and raw */ if(i<3) { time = utc2gpst(timeget()); server.raw[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time; server.rtcm[i].time = stream_type[i] == STR_FILE ? strgettime(server.stream+i) : time; } } /* sync input streams */ strsync(server.stream, server.stream+1); strsync(server.stream, server.stream+2); /* write start commands to input streams */ for(int i=0 ; i<3 ; i++) { if(cmds[i]) strsendcmd(server.stream+i, cmds[i]); } /* write solution header to solution streams */ for(int i=3 ; i<5 ; i++) { unsigned char buff[1024]; int n; n = outsolheads(buff, server.solopt+i-3); strwrite(server.stream+i, buff, n); } //******************************************************** obs_t obs; obsd_t data[MAXOBS*2]; server.state=1; obs.data=data; double tt; unsigned int tick; int fobs[3] = {0}; server.tick = tickget(); ROS_DEBUG("RTK -- Initialization complete."); ros::Rate r(rate); while(ros::ok()) { tick = tickget(); unsigned char *p = server.buff[RTK_ROBOT]+server.nb[RTK_ROBOT]; unsigned char *q = server.buff[RTK_ROBOT]+server.buffsize; ROS_DEBUG("RTK -- Getting data from GPS..."); /* read receiver raw/rtcm data from input stream */ n = strread(server.stream, p, q-p); /* write receiver raw/rtcm data to log stream */ strwrite(server.stream+5, p, n); server.nb[RTK_ROBOT] += n; /* save peek buffer */ rtksvrlock(&server); n = n < server.buffsize - server.npb[RTK_ROBOT] ? n : server.buffsize - server.npb[RTK_ROBOT]; memcpy(server.pbuf[RTK_ROBOT] + server.npb[RTK_ROBOT], p, n); server.npb[RTK_ROBOT] += n; rtksvrunlock(&server); ROS_DEBUG("RTK -- Decoding GPS data..."); /* decode data */ fobs[RTK_ROBOT] = decoderaw(&server, RTK_ROBOT); fobs[RTK_BASE_STATION] = decoderaw(&server, RTK_BASE_STATION); ROS_DEBUG("RTK -- Got %d observations.", fobs[RTK_ROBOT]); /* for each rover observation data */ for(int i=0 ; i<fobs[RTK_ROBOT] ; i++) { obs.n = 0; for(int j=0 ; j<server.obs[RTK_ROBOT][i].n && obs.n<MAXOBS*2 ; j++) { obs.data[obs.n++] = server.obs[RTK_ROBOT][i].data[j]; } for(int j=0 ; j<server.obs[1][0].n && obs.n<MAXOBS*2 ; j++) { obs.data[obs.n++] = server.obs[1][0].data[j]; } ROS_DEBUG("RTK -- Calculating RTK positioning..."); /* rtk positioning */ rtksvrlock(&server); rtkpos(&server.rtk, obs.data, obs.n, &server.nav); rtksvrunlock(&server); sensor_msgs::NavSatFix gps_msg; gps_msg.header.stamp = ros::Time::now(); gps_msg.header.frame_id = gps_frame_id; rtk_msgs::Status status_msg; status_msg.stamp = gps_msg.header.stamp; if(server.rtk.sol.stat != SOLQ_NONE) { /* adjust current time */ tt = (int)(tickget()-tick)/1000.0+DTTOL; timeset(gpst2utc(timeadd(server.rtk.sol.time,tt))); /* write solution */ unsigned char buff[1024]; n = outsols(buff, &server.rtk.sol, server.rtk.rb, server.solopt); if(n==141 && buff[0]>'0' && buff[0]<'9') { int ano,mes,dia,horas,minutos,Q,nsat; double segundos,lat,longi,alt,sde,sdn,sdu,sdne,sdeu,sdun; sscanf((const char *)(buff),"%d/%d/%d %d:%d:%lf %lf %lf %lf %d %d %lf %lf %lf %lf %lf %lf", &ano, &mes, &dia, &horas, &minutos, &segundos, &lat, &longi, &alt, &Q, &nsat, &sdn, &sde, &sdu, &sdne, &sdeu, &sdun); gps_msg.latitude = lat; gps_msg.longitude = longi; gps_msg.altitude = alt; gps_msg.position_covariance_type = sensor_msgs::NavSatFix::COVARIANCE_TYPE_KNOWN; gps_msg.position_covariance[0] = sde + ecef_base_station.position_covariance[0]; gps_msg.position_covariance[1] = sdne + ecef_base_station.position_covariance[1]; gps_msg.position_covariance[2] = sdeu + ecef_base_station.position_covariance[2]; gps_msg.position_covariance[3] = sdne + ecef_base_station.position_covariance[3]; gps_msg.position_covariance[4] = sdn + ecef_base_station.position_covariance[4]; gps_msg.position_covariance[5] = sdun + ecef_base_station.position_covariance[5]; gps_msg.position_covariance[6] = sdeu + ecef_base_station.position_covariance[6]; gps_msg.position_covariance[7] = sdun + ecef_base_station.position_covariance[7]; gps_msg.position_covariance[8] = sdu + ecef_base_station.position_covariance[8]; gps_msg.status.status = Q==5 ? sensor_msgs::NavSatStatus::STATUS_FIX : sensor_msgs::NavSatStatus::STATUS_GBAS_FIX; gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; status_msg.fix_quality = Q; status_msg.number_of_satellites = nsat; } } else { gps_msg.status.status = sensor_msgs::NavSatStatus::STATUS_NO_FIX; gps_msg.status.service = sensor_msgs::NavSatStatus::SERVICE_GPS; } ROS_DEBUG("RTK -- Publishing ROS msg..."); gps_pub.publish(gps_msg); status_pub.publish(status_msg); } ros::spinOnce(); r.sleep(); } return(0); }
/* decode ublox rxm-raw: raw measurement data --------------------------------*/ static int decode_rxmraw(raw_t *raw) { gtime_t time; double tow,tt,tadj=0.0,toff=0.0,tn; int i,j,prn,sat,n=0,nsat,week; unsigned char *p=raw->buff+6; char *q,tstr[32]; trace(4,"decode_rxmraw: len=%d\n",raw->len); /* time tag adjustment option (-tadj) */ if ((q=strstr(raw->opt,"-tadj"))) { sscanf(q,"-tadj %lf",&tadj); } tow =U4(p ); week=U2(p+4); nsat=U1(p+6); if (raw->len<12+24*nsat) { trace(2,"ubx rxmraw length error: len=%d nsat=%d\n",raw->len,nsat); return -1; } time=gpst2time(week,tow*0.001); /* time tag adjustment */ if (tadj>0.0) { tn=time2gpst(time,&week)/tadj; toff=(tn-floor(tn+0.5))*tadj; time=timeadd(time,-toff); } if (fabs(tt=timediff(time,raw->time))<=1e-3) { time2str(time,tstr,3); trace(2,"ubx rxmraw time tag duplicated: time=%s\n",tstr); return 0; } for (i=0,p+=8;i<nsat&&i<MAXOBS;i++,p+=24) { raw->obs.data[n].time=time; raw->obs.data[n].L[0] =R8(p )-toff*FREQ1; raw->obs.data[n].P[0] =R8(p+ 8)-toff*CLIGHT; raw->obs.data[n].D[0] =R4(p+16); prn =U1(p+20); raw->obs.data[n].SNR[0]=(unsigned char)(I1(p+22)*4.0+0.5); raw->obs.data[n].LLI[0]=U1(p+23); raw->obs.data[n].code[0]=CODE_L1C; /* phase polarity flip option (-invcp) */ if (strstr(raw->opt,"-invcp")) { raw->obs.data[n].L[0]=-raw->obs.data[n].L[0]; } if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) { trace(2,"ubx rxmraw sat number error: prn=%d\n",prn); continue; } raw->obs.data[n].sat=sat; if (raw->obs.data[n].LLI[0]&1) raw->lockt[sat-1][0]=0.0; else if (tt<0.0||10.0<tt) raw->lockt[sat-1][0]=0.0; else raw->lockt[sat-1][0]+=tt; for (j=1;j<NFREQ;j++) { raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0; raw->obs.data[n].D[j]=0.0; raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0; raw->obs.data[n].code[j]=CODE_NONE; } n++; } raw->time=time; raw->obs.n=n; return 1; }
/* generate simulated observation data ---------------------------------------*/ static int simobs(gtime_t ts, gtime_t te, double tint, const double *rr, nav_t *nav, obs_t *obs, int opt) { gtime_t time; obsd_t data[MAXSAT]={{{0}}}; double pos[3],rs[3*MAXSAT],dts[MAXSAT],r,e[3],azel[2]; double ecp[MAXSAT][NFREQ]={{0}},epr[MAXSAT][NFREQ]={{0}}; double snr[MAXSAT][NFREQ]={{0}},ers[MAXSAT][3]={{0}}; double iono,trop,fact,cp,pr,dtr=0.0,rref[3],bl; int i,j,k,n,ns,amb[MAXSAT][NFREQ]={{0}},sys,prn; char s[64]; double pref[]={36.106114294,140.087190410,70.3010}; /* ref station */ trace(3,"simobs:nnav=%d ngnav=%d\n",nav->n,nav->ng); for (i=0;i<2;i++) pref[i]*=D2R; pos2ecef(pref,rref); for (i=0;i<3;i++) rref[i]-=rr[i]; bl=norm(rref,3)/1E4; /* baseline (10km) */ srand(0); /* ephemeris error */ for (i=0;i<MAXSAT;i++) { data[i].sat=i+1; data[i].P[0]=2E7; for (j=0;j<3;j++) ers[i][j]=randn(0.0,erreph); } srand(tickget()); ecef2pos(rr,pos); n=(int)(timediff(te,ts)/tint+1.0); for (i=0;i<n;i++) { time=timeadd(ts,tint*i); time2str(time,s,0); for (j=0;j<MAXSAT;j++) data[j].time=time; for (j=0;j<3;j++) { /* iteration for pseudorange */ satpos(time,data,MAXSAT,nav,rs,dts); for (k=0;k<MAXSAT;k++) { if ((r=geodist(rs+k*3,rr,e))<=0.0) continue; data[k].P[0]=r+CLIGHT*(dtr-dts[k]); } } satpos(time,data,MAXSAT,nav,rs,dts); for (j=ns=0;j<MAXSAT;j++) { /* add ephemeris error */ for (k=0;k<3;k++) rs[k+j*3]+=ers[j][k]; if ((r=geodist(rs+j*3,rr,e))<=0.0) continue; satazel(pos,e,azel); if (azel[1]<minel*D2R) continue; iono=ionmodel(time,nav->ion,pos,azel); trop=tropmodel(pos,azel,0.3); /* add ionospheric error */ iono+=errion*bl*ionmapf(pos,azel); snrmodel(azel,snr[j]); errmodel(azel,snr[j],ecp[j],epr[j]); sys=satsys(data[j].sat,&prn); for (k=0;k<NFREQ;k++) { data[j].L[k]=data[j].P[k]=0.0; data[j].SNR[k]=0; data[j].LLI[k]=0; if (sys==SYS_GPS) { if (k>=3) continue; /* no L5a/L5b in gps */ if (k>=2&&!gpsblock[prn-1]) continue; /* no L5 in block II */ } else if (sys==SYS_GLO) { if (k>=3) continue; } else if (sys==SYS_GAL) { if (k==1) continue; /* no L2 in galileo */ } else continue; /* generate observation data */ fact=lam[k]*lam[k]/lam[0]/lam[0]; cp=r+CLIGHT*(dtr-dts[j])-fact*iono+trop+ecp[j][k]; pr=r+CLIGHT*(dtr-dts[j])+fact*iono+trop+epr[j][k]; if (amb[j][k]==0) amb[j][k]=(int)(-cp/lam[k]); data[j].L[k]=cp/lam[k]+amb[j][k]; data[j].P[k]=pr; data[j].SNR[k]=(unsigned char)snr[j][k]; data[j].LLI[k]=data[j].SNR[k]<slipthres?1:0; } if (obs->nmax<=obs->n) { if (obs->nmax==0) obs->nmax=65532; else obs->nmax+=65532; if (!(obs->data=(obsd_t *)realloc(obs->data,sizeof(obsd_t)*obs->nmax))) { fprintf(stderr,"malloc error\n"); return 0; } } obs->data[obs->n++]=data[j]; ns++; } fprintf(stderr,"time=%s nsat=%2d\r",s,ns); } fprintf(stderr,"\n"); return 1; }
/* set approx position -------------------------------------------------------*/ static void setapppos(strfile_t *str, rnxopt_t *opt) { prcopt_t prcopt=prcopt_default; sol_t sol={{0}}; char msg[128]; prcopt.navsys=opt->navsys; /* point positioning with last obs data */ #ifdef WAAS_STUDY if (!pntpos(str->obs->data,str->obs->n,str->nav,&prcopt,&sol,NULL,NULL, NULL,msg)) { #else if (!pntpos(str->obs->data,str->obs->n,str->nav,&prcopt,&sol,NULL,NULL, msg)) { #endif trace(2,"point position error (%s)\n",msg); return; } matcpy(opt->apppos,sol.rr,3,1); } /* show status message -------------------------------------------------------*/ static int showstat(int sess, gtime_t ts, gtime_t te, int *n) { const char type[]="ONGHQLSE"; char msg[1024]="",*p=msg,s[64]; int i; if (sess>0) { p+=sprintf(p,"(%d) ",sess); } if (ts.time!=0) { time2str(ts,s,0); p+=sprintf(p,"%s",s); } if (te.time!=0&&timediff(te,ts)>0.9) { time2str(te,s,0); p+=sprintf(p,"-%s",s+5); } p+=sprintf(p,": "); for (i=0;i<NOUTFILE+1;i++) { if (n[i]==0) continue; p+=sprintf(p,"%c=%d%s",type[i],n[i],i<NOUTFILE?" ":""); } return showmsg(msg); } /* rinex converter for single-session ----------------------------------------*/ static int convrnx_s(int sess, int format, rnxopt_t *opt, const char *file, char **ofile) { FILE *ofp[NOUTFILE]={NULL}; strfile_t *str; gtime_t ts={0},te={0},tend={0},time={0}; unsigned char slips[MAXSAT][NFREQ+NEXOBS]={{0}}; int i,j,nf,type,n[NOUTFILE+1]={0},abort=0; char path[1024],*paths[NOUTFILE],s[NOUTFILE][1024]; char *epath[MAXEXFILE]={0},*staid=*opt->staid?opt->staid:"0000"; trace(3,"convrnx_s: sess=%d format=%d file=%s ofile=%s %s %s %s %s %s %s\n", sess,format,file,ofile[0],ofile[1],ofile[2],ofile[3],ofile[4], ofile[5],ofile[6]); /* replace keywords in input file */ if (reppath(file,path,opt->ts,staid,"")<0) { showmsg("no time for input file: %s",file); return 0; } /* expand wild-cards in input file */ for (i=0;i<MAXEXFILE;i++) { if (!(epath[i]=(char *)malloc(1024))) { for (i=0;i<MAXEXFILE;i++) free(epath[i]); return 0; } } nf=expath(path,epath,MAXEXFILE); if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) time=opt->trtcm; if (opt->scanobs) { /* scan observation types */ if (!scan_obstype(format,epath[0],opt,&time)) return 0; } else { /* set observation types by format */ set_obstype(format,opt); } if (!(str=gen_strfile(format,opt->rcvopt,time))) { for (i=0;i<MAXEXFILE;i++) free(epath[i]); return 0; } time=opt->ts.time?opt->ts:(time.time?timeadd(time,TSTARTMARGIN):time); /* replace keywords in output file */ for (i=0;i<NOUTFILE;i++) { paths[i]=s[i]; if (reppath(ofile[i],paths[i],time,staid,"")<0) { showmsg("no time for output path: %s",ofile[i]); for (i=0;i<MAXEXFILE;i++) free(epath[i]); free_strfile(str); return 0; } } /* open output files */ if (!openfile(ofp,paths,path,opt,str->nav)) { for (i=0;i<MAXEXFILE;i++) free(epath[i]); free_strfile(str); return 0; } for (i=0;i<nf&&!abort;i++) { /* open stream file */ if (!open_strfile(str,epath[i])) continue; /* input message */ for (j=0;(type=input_strfile(str))>=-1;j++) { if (j%11==1&&(abort=showstat(sess,te,te,n))) break; /* avioid duplicated if overlapped data */ if (tend.time&&timediff(str->time,tend)<=0.0) continue; /* convert message */ switch (type) { case 1: convobs(ofp,opt,str,n,slips); break; case 2: convnav(ofp,opt,str,n); break; case 3: convsbs(ofp,opt,str,n); break; case 31: convlex(ofp,opt,str,n); break; case -1: n[NOUTFILE]++; break; /* error */ } te=str->time; if (ts.time==0) ts=te; /* set approx position */ if (type==1&&!opt->autopos&&norm(opt->apppos,3)<=0.0) { setapppos(str,opt); } if (opt->te.time&&timediff(te,opt->te)>10.0) break; } /* close stream file */ close_strfile(str); tend=te; /* end time of a file */ } /* set receiver and antenna information to option */ if (format==STRFMT_RTCM2||format==STRFMT_RTCM3) { rtcm2opt(&str->rtcm,opt); } else if (format==STRFMT_RINEX) { rnx2opt(&str->rnx,opt); } /* close output files */ closefile(ofp,opt,str->nav); /* remove empty output files */ for (i=0;i<NOUTFILE;i++) { if (ofp[i]&&n[i]<=0) remove(ofile[i]); } if (ts.time>0) showstat(sess,ts,te,n); for (i=0;i<MAXEXFILE;i++) free(epath[i]); free_strfile(str); if (opt->tstart.time==0) opt->tstart=opt->ts; if (opt->tend .time==0) opt->tend =opt->te; return abort?-1:1; }
/* main ----------------------------------------------------------------------*/ int main(int argc, char **argv) { FILE *fp=NULL; nav_t nav={0}; lex_t lex={0}; gtime_t t0,time; double ep0[]={2000,1,1,0,0,0},tspan=24.0,tint=7200,dpos=1; double rpos[]={35,137}; char *ifile="",*ofile=""; int i,trl=0,index=0,nlat=45,nlon=45; t0=epoch2time(ep0); for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-t0")&&i+2<argc) { if (sscanf(argv[++i],"%lf/%lf/%lf",ep0 ,ep0+1,ep0+2)<3|| sscanf(argv[++i],"%lf:%lf:%lf",ep0+3,ep0+4,ep0+5)<1) { fprintf(stderr,"invalid time\n"); return -1; } } else if (!strcmp(argv[i],"-ts")&&i+1<argc) tspan=atof(argv[++i]); else if (!strcmp(argv[i],"-ti")&&i+1<argc) tint =atof(argv[++i]); else if (!strcmp(argv[i],"-u" )&&i+2<argc) { rpos[0]=atof(argv[++i]); rpos[1]=atof(argv[++i]); } else if (!strcmp(argv[i],"-n" )&&i+2<argc) { nlat=atoi(argv[++i]); nlon=atoi(argv[++i]); } else if (!strcmp(argv[i],"-d" )&&i+1<argc) dpos =atof(argv[++i]); else if (!strcmp(argv[i],"-o" )&&i+1<argc) ofile=argv[++i]; else if (!strcmp(argv[i],"-x" )&&i+1<argc) trl =atof(argv[++i]); else ifile=argv[i]; } if (trl>0) { traceopen("diffeph.trace"); tracelevel(trl); } t0=epoch2time(ep0); if (!lexreadmsg(ifile,0,&lex)) { fprintf(stderr,"file read error: %s\n",ifile); return -1; } if (!(fp=fopen(ofile,"w"))) { fprintf(stderr,"file open error: %s\n",ofile); return -1; } fprintf(fp,"epoch=[%.0f %.0f %.0f %.0f %.0f %.0f];\n", ep0[0],ep0[1],ep0[2],ep0[3],ep0[4],ep0[5]); for (i=0;i<(int)(tspan*3600.0/tint);i++) { time=timeadd(t0,tint*i); fprintf(stderr,"time=%s\r",time_str(time,0)); index=updatelex(index,time,&lex,&nav); printtec(i+1,time,tint*i,&nav,rpos,nlat,nlon,dpos,fp); } fclose(fp); if (trl>0) traceclose(); return 0; }
// generate visibility data ---------------------------------------------------- void __fastcall TPlot::GenVisData(void) { gtime_t time,ts,te; obsd_t data={{0}}; sta_t sta={0}; double tint,r,pos[3],rr[3],rs[6],e[3],azel[2]; int i,j,nobs=0; char name[16]; trace(3,"GenVisData\n"); ClearObs(); SimObs=1; ts=TimeStart; te=TimeEnd; tint=TimeInt; matcpy(pos,OOPos,3,1); pos2ecef(pos,rr); ReadWaitStart(); ShowLegend(NULL); ShowMsg("generating satellite visibility..."); Application->ProcessMessages(); for (time=ts;timediff(time,te)<=0.0;time=timeadd(time,tint)) { for (i=0;i<MAXSAT;i++) { satno2id(i+1,name); if (!tle_pos(time,name,"","",&TLEData,NULL,rs)) continue; if ((r=geodist(rs,rr,e))<=0.0) continue; if (satazel(pos,e,azel)<=0.0) continue; if (Obs.n>=Obs.nmax) { Obs.nmax=Obs.nmax<=0?4096:Obs.nmax*2; Obs.data=(obsd_t *)realloc(Obs.data,sizeof(obsd_t)*Obs.nmax); if (!Obs.data) { Obs.n=Obs.nmax=0; break; } } data.time=time; data.sat=i+1; for (j=0;j<NFREQ;j++) { data.P[j]=data.L[j]=0.0; data.code[j]=CODE_NONE; } data.code[0]=CODE_L1C; Obs.data[Obs.n++]=data; } if (++nobs>=MAX_SIMOBS) break; } if (Obs.n<=0) { ReadWaitEnd(); ShowMsg("no satellite visibility"); return; } UpdateObs(nobs); Caption="Satellite Visibility (Predicted)"; BtnSol1->Down=true; time2gpst(Obs.data[0].time,&Week); SolIndex[0]=SolIndex[1]=ObsIndex=0; if (PlotType<PLOT_OBS||PLOT_DOP<PlotType) { UpdateType(PLOT_OBS); } else { UpdatePlotType(); } FitTime(); ReadWaitEnd(); UpdateObsType(); UpdateTime(); UpdatePlot(); }
/* generate data qc page -----------------------------------------------------*/ static int gen_page3(gtime_t time, const char *temp, const char *page) { FILE *ifp,*ofp; gtime_t ts,te; double ep[6],hour[24],mp1[MAXSTA][24]= {{0}},mp2[MAXSTA][24]= {{0}}; double slip[MAXSTA][24]= {{0}},val1[MAXSTA],val2[MAXSTA],val3[MAXSTA]; char buff[1024],file[1024],*sta[MAXSTA],str[MAXSTA][32]; int i,j,n,nsta=0,sec=0; ts=timeadd(time,-90000.0); te=timeadd(time, -3600.0); for (i=0; i<MAXSTA; i++) sta[i]=str[i]; for (i=0; i<24; i++) { time2epoch(timeadd(ts,i*3600.0),ep); sprintf(file,QC_FILE,ep[0],ep[1],ep[2],ep[3]); if (!(n=read_qc(file,sta,val1,val2,val3))) continue; for (j=0; j<n; j++) { mp1 [j][i]=val1[j]; mp2 [j][i]=val2[j]; slip[j][i]=val3[j]; } hour[i]=ep[3]; nsta=n; } if (!(ifp=fopen(temp,"r"))) { fprintf(stderr,"file open error: %s\n",temp); return 0; } if (!(ofp=fopen(page,"w"))) { fprintf(stderr,"file open error: %s\n",page); fclose(ifp); return 0; } while (fgets(buff,sizeof(buff),ifp)) { if (strstr(buff,"@DATE START" )) sec=1; else if (strstr(buff,"@TABLE0 START")) sec=2; else if (strstr(buff,"@TABLE1 START")) sec=3; else if (strstr(buff,"@TABLE2 START")) sec=4; else if (strstr(buff,"@DATE END" )) sec=0; else if (strstr(buff,"@TABLE0 END" )) sec=0; else if (strstr(buff,"@TABLE1 END" )) sec=0; else if (strstr(buff,"@TABLE2 END" )) sec=0; switch (sec) { case 0: fputs(buff,ofp); break; case 1: out_date(ofp,ts,te); break; case 2: out_table1(ofp,sta,nsta,hour,mp1 ,3); break; case 3: out_table1(ofp,sta,nsta,hour,mp2 ,3); break; case 4: out_table1(ofp,sta,nsta,hour,slip,0); break; } } fclose(ifp); fclose(ofp); return 1; }
/* decode NVS xf5-raw: raw measurement data ----------------------------------*/ static int decode_xf5raw(raw_t *raw) { gtime_t time; double tadj=0.0,toff=0.0,tn; int dTowInt; double dTowUTC, dTowGPS, dTowFrac, L1, P1, D1; double gpsutcTimescale; unsigned char rcvTimeScaleCorr, sys, carrNo; int i,j,prn,sat,n=0,nsat,week; unsigned char *p=raw->buff+2; char *q,tstr[32],flag; trace(4,"decode_xf5raw: len=%d\n",raw->len); /* time tag adjustment option (-TADJ) */ if ((q=strstr(raw->opt,"-tadj"))) { sscanf(q,"-TADJ=%lf",&tadj); } dTowUTC =R8(p); week = U2(p+8); gpsutcTimescale = R8(p+10); /* glonassutcTimescale = R8(p+18); */ rcvTimeScaleCorr = I1(p+26); /* check gps week range */ if (week>=4096) { trace(2,"nvs xf5raw obs week error: week=%d\n",week); return -1; } week=adjgpsweek(week); if ((raw->len - 31)%30) { /* Message length is not correct: there could be an error in the stream */ trace(2,"nvs xf5raw len=%d seems not be correct\n",raw->len); return -1; } nsat = (raw->len - 31)/30; dTowGPS = dTowUTC + gpsutcTimescale; /* Tweak pseudoranges to allow Rinex to represent the NVS time of measure */ dTowInt = 10.0*floor((dTowGPS/10.0)+0.5); dTowFrac = dTowGPS - (double) dTowInt; time=gpst2time(week, dTowInt*0.001); /* time tag adjustment */ if (tadj>0.0) { tn=time2gpst(time,&week)/tadj; toff=(tn-floor(tn+0.5))*tadj; time=timeadd(time,-toff); } /* check time tag jump */ if (raw->time.time&&fabs(timediff(time,raw->time))>86400.0) { time2str(time,tstr,3); trace(2,"nvs xf5raw time tag jump error: time=%s\n",tstr); return 0; } if (fabs(timediff(time,raw->time))<=1e-3) { time2str(time,tstr,3); trace(2,"nvs xf5raw time tag duplicated: time=%s\n",tstr); return 0; } for (i=0,p+=27;(i<nsat) && (n<MAXOBS); i++,p+=30) { raw->obs.data[n].time = time; sys = (U1(p)==1)?SYS_GLO:((U1(p)==2)?SYS_GPS:((U1(p)==4)?SYS_SBS:SYS_NONE)); prn = U1(p+1); if (sys == SYS_SBS) prn += 120; /* Correct this */ if (!(sat=satno(sys,prn))) { trace(2,"nvs xf5raw satellite number error: sys=%d prn=%d\n",sys,prn); continue; } carrNo = I1(p+2); L1 = R8(p+ 4); P1 = R8(p+12); D1 = R8(p+20); /* check range error */ if (L1<-1E10||L1>1E10||P1<-1E10||P1>1E10||D1<-1E5||D1>1E5) { trace(2,"nvs xf5raw obs range error: sat=%2d L1=%12.5e P1=%12.5e D1=%12.5e\n", sat,L1,P1,D1); continue; } raw->obs.data[n].SNR[0]=(unsigned char)(I1(p+3)*4.0+0.5); if (sys==SYS_GLO) { raw->obs.data[n].L[0] = L1 - toff*(FREQ1_GLO+DFRQ1_GLO*carrNo); } else { raw->obs.data[n].L[0] = L1 - toff*FREQ1; } raw->obs.data[n].P[0] = (P1-dTowFrac)*CLIGHT*0.001 - toff*CLIGHT; /* in ms, needs to be converted */ raw->obs.data[n].D[0] = (float)D1; /* set LLI if meas flag 4 (carrier phase present) off -> on */ flag=U1(p+28); raw->obs.data[n].LLI[0]=(flag&0x08)&&!(raw->halfc[sat-1][0]&0x08)?1:0; raw->halfc[sat-1][0]=flag; #if 0 if (raw->obs.data[n].SNR[0] > 160) { time2str(time,tstr,3); trace(2,"%s, obs.data[%d]: SNR=%.3f LLI=0x%02x\n", tstr, n, (raw->obs.data[n].SNR[0])/4.0, U1(p+28) ); } #endif raw->obs.data[n].code[0] = CODE_L1C; raw->obs.data[n].sat = sat; for (j=1;j<NFREQ+NEXOBS;j++) { raw->obs.data[n].L[j]=raw->obs.data[n].P[j]=0.0; raw->obs.data[n].D[j]=0.0; raw->obs.data[n].SNR[j]=raw->obs.data[n].LLI[j]=0; raw->obs.data[n].code[j]=CODE_NONE; } n++; } raw->time=time; raw->obs.n=n; return 1; }
/* post-processing positioning ------------------------------------------------- * post-processing positioning * args : gtime_t ts I processing start time (ts.time==0: no limit) * : gtime_t te I processing end time (te.time==0: no limit) * double ti I processing interval (s) (0:all) * double tu I processing unit time (s) (0:all) * prcopt_t *popt I processing options * solopt_t *sopt I solution options * filopt_t *fopt I file options * char **infile I input files (see below) * int n I number of input files * char *outfile I output file ("":stdout, see below) * char *rov I rover id list (separated by " ") * char *base I base station id list (separated by " ") * return : status (0:ok,0>:error,1:aborted) * notes : input files should contain observation data, navigation data, precise * ephemeris/clock (optional), sbas log file (optional), ssr message * log file (optional) and tec grid file (optional). only the first * observation data file in the input files is recognized as the rover * data. * * the type of an input file is recognized by the file extention as ] * follows: * .sp3,.SP3,.eph*,.EPH*: precise ephemeris (sp3c) * .sbs,.SBS,.ems,.EMS : sbas message log files (rtklib or ems) * .lex,.LEX : qzss lex message log files * .rtcm3,.RTCM3 : ssr message log files (rtcm3) * .*i,.*I : tec grid files (ionex) * others : rinex obs, nav, gnav, hnav, qnav or clock * * inputs files can include wild-cards (*). if an file includes * wild-cards, the wild-card expanded multiple files are used. * * inputs files can include keywords. if an file includes keywords, * the keywords are replaced by date, time, rover id and base station * id and multiple session analyses run. refer reppath() for the * keywords. * * the output file can also include keywords. if the output file does * not include keywords. the results of all multiple session analyses * are output to a single output file. * * ssr corrections are valid only for forward estimation. *-----------------------------------------------------------------------------*/ extern int postpos(gtime_t ts, gtime_t te, double ti, double tu, const prcopt_t *popt, const solopt_t *sopt, const filopt_t *fopt, char **infile, int n, char *outfile, const char *rov, const char *base) { gtime_t tts,tte,ttte; double tunit,tss; int i,j,k,nf,stat=0,week,flag=1,index[MAXINFILE]={0}; char *ifile[MAXINFILE],ofile[1024],*ext; trace(3,"postpos : ti=%.0f tu=%.0f n=%d outfile=%s\n",ti,tu,n,outfile); /* open processing session */ if (!openses(popt,sopt,fopt,&navs,&pcvss,&pcvsr)) return -1; if (ts.time!=0&&te.time!=0&&tu>=0.0) { if (timediff(te,ts)<0.0) { showmsg("error : no period"); closeses(&navs,&pcvss,&pcvsr); return 0; } for (i=0;i<MAXINFILE;i++) { if (!(ifile[i]=(char *)malloc(1024))) { for (;i>=0;i--) free(ifile[i]); closeses(&navs,&pcvss,&pcvsr); return -1; } } if (tu==0.0||tu>86400.0*MAXPRCDAYS) tu=86400.0*MAXPRCDAYS; settspan(ts,te); tunit=tu<86400.0?tu:86400.0; tss=tunit*(int)floor(time2gpst(ts,&week)/tunit); for (i=0;;i++) { /* for each periods */ tts=gpst2time(week,tss+i*tu); tte=timeadd(tts,tu-DTTOL); if (timediff(tts,te)>0.0) break; if (timediff(tts,ts)<0.0) tts=ts; if (timediff(tte,te)>0.0) tte=te; strcpy(proc_rov ,""); strcpy(proc_base,""); if (checkbrk("reading : %s",time_str(tts,0))) { stat=1; break; } for (j=k=nf=0;j<n;j++) { ext=strrchr(infile[j],'.'); if (ext&&(!strcmp(ext,".rtcm3")||!strcmp(ext,".RTCM3"))) { strcpy(ifile[nf++],infile[j]); } else { /* include next day precise ephemeris or rinex brdc nav */ ttte=tte; if (ext&&(!strcmp(ext,".sp3")||!strcmp(ext,".SP3")|| !strcmp(ext,".eph")||!strcmp(ext,".EPH"))) { ttte=timeadd(ttte,3600.0); } else if (strstr(infile[j],"brdc")) { ttte=timeadd(ttte,7200.0); } nf+=reppaths(infile[j],ifile+nf,MAXINFILE-nf,tts,ttte,"",""); } while (k<nf) index[k++]=j; if (nf>=MAXINFILE) { trace(2,"too many input files. trancated\n"); break; } } if (!reppath(outfile,ofile,tts,"","")&&i>0) flag=0; /* execute processing session */ stat=execses_b(tts,tte,ti,popt,sopt,fopt,flag,ifile,index,nf,ofile, rov,base); if (stat==1) break; } for (i=0;i<MAXINFILE;i++) free(ifile[i]); } else if (ts.time!=0) { for (i=0;i<n&&i<MAXINFILE;i++) { if (!(ifile[i]=(char *)malloc(1024))) { for (;i>=0;i--) free(ifile[i]); return -1; } reppath(infile[i],ifile[i],ts,"",""); index[i]=i; } reppath(outfile,ofile,ts,"",""); /* execute processing session */ stat=execses_b(ts,te,ti,popt,sopt,fopt,1,ifile,index,n,ofile,rov, base); for (i=0;i<n&&i<MAXINFILE;i++) free(ifile[i]); } else { for (i=0;i<n;i++) index[i]=i; /* execute processing session */ stat=execses_b(ts,te,ti,popt,sopt,fopt,1,infile,index,n,outfile,rov, base); } /* close processing session */ closeses(&navs,&pcvss,&pcvsr); return stat; }
int main(int argc, char *argv[]) { struct pollfd pfd[1]; char *ep, *cp; int on = 1; int send_time = 180; /* Default time, 180 seconds (3 minutes) */ struct sockaddr_in m_sin; uid_t unpriv_uid; gid_t unpriv_gid; long tmp; time_t delta = 0; struct timeval next, now; if (getuid()) errx(1, "not super user"); run_as(&unpriv_uid, &unpriv_gid); argv++; argc--; while (argc > 0 && *argv[0] == '-') { if (strcmp(*argv, "-m") == 0) { if (argc > 1 && *(argv + 1)[0] != '-') { /* Argument has been given */ argv++, argc--; multicast_mode = SCOPED_MULTICAST; tmp = strtol(*argv, &ep, 10); if (*ep != '\0' || tmp < INT_MIN || tmp > INT_MAX) errx(1, "invalid ttl: %s", *argv); multicast_scope = (int)tmp; if (multicast_scope > MAX_MULTICAST_SCOPE) errx(1, "ttl must not exceed %u", MAX_MULTICAST_SCOPE); } else multicast_mode = PER_INTERFACE_MULTICAST; } else if (strcmp(*argv, "-g") == 0) { if (argc > 1 && *(argv + 1)[0] != '-') { argv++, argc--; send_time = (int)strtol(*argv, &ep, 10); if (send_time <= 0) errx(1, "time must be greater than 0"); if (ep[0] != '\0') { if (ep[1] != '\0') errx(1, "invalid argument: %s", *argv); if (*ep == 'M' || *ep == 'm') { /* Time in minutes. */ send_time *= 60; } else errx(1, "invalid argument: %s", *argv); } if (send_time > 180) errx(1, "cannot be greater than 180 seconds (3 minutes)"); } else errx(1, "missing argument"); } else if (strcmp(*argv, "-i") == 0) insecure_mode = 1; else if (strcmp(*argv, "-l") == 0) quiet_mode = 1; else if (strcmp(*argv, "-p") == 0) iff_flag = 0; else usage(); argv++, argc--; } if (argc > 0) usage(); #ifndef DEBUG struct pidfh *pfh = NULL; pfh = pidfile_open(NULL, 600, NULL); daemon(1, 0); pidfile_write(pfh); #endif signal(SIGHUP, hup); openlog("rwhod", LOG_PID, LOG_DAEMON); sp = getservbyname("who", "udp"); if (sp == NULL) quit("udp/who: unknown service", WITHOUT_ERRNO); if (chdir(_PATH_RWHODIR) < 0) quit(_PATH_RWHODIR, WITH_ERRNO); /* * Establish host name as returned by system. */ if (gethostname(myname, sizeof(myname)) < 0) quit("gethostname", WITH_ERRNO); if ((cp = strchr(myname, '.')) != NULL) *cp = '\0'; strlcpy(mywd.wd_hostname, myname, sizeof(mywd.wd_hostname)); utmpf = open(_PATH_UTMP, O_RDONLY|O_CREAT, 0644); if (utmpf < 0) quit(_PATH_UTMP, WITH_ERRNO); getboottime(); if ((s = socket(AF_INET, SOCK_DGRAM, 0)) < 0) quit("socket", WITH_ERRNO); if (setsockopt(s, SOL_SOCKET, SO_BROADCAST, &on, sizeof(on)) < 0) quit("setsockopt SO_BROADCAST", WITH_ERRNO); memset(&m_sin, 0, sizeof(m_sin)); m_sin.sin_len = sizeof(m_sin); m_sin.sin_family = AF_INET; m_sin.sin_port = sp->s_port; if (bind(s, (struct sockaddr *)&m_sin, sizeof(m_sin)) < 0) quit("bind", WITH_ERRNO); setgid(unpriv_gid); setgroups(1, &unpriv_gid); /* XXX BOGUS groups[0] = egid */ setuid(unpriv_uid); if (!configure(s)) exit(1); if (!quiet_mode) { send_host_information(); delta = send_time; gettimeofday(&now, NULL); timeadd(&now, delta, &next); } pfd[0].fd = s; pfd[0].events = POLLIN; for (;;) { int n; n = poll(pfd, 1, 1000); if (onsighup) { onsighup = 0; getboottime(); } if (n == 1) handleread(s); if (!quiet_mode) { gettimeofday(&now, NULL); if (now.tv_sec > next.tv_sec) { send_host_information(); timeadd(&now, delta, &next); } } } }
/* estimate receiver position ------------------------------------------------*/ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, prcopt_t *opt, sol_t *sol, double *azel, int *vsat, double *resp, char *msg, rtk_t *rtk) { double x[NX]={0},dx[NX],Q[NX*NX],sig; int i,j,k,info,stat,nv,ns; double rms = 0; //added by yuan; double RMS = 0; int nv_ = 0; double temp = 0; int kk = 0; double v[(MAXOBS_+4)*1];double H[(MAXOBS_+4)*135];double var[MAXOBS_+4*1]; for (i=0;i<3;i++) x[i]=sol->rr[i]; for (i = 0; i < MAXITR; i++) { /* pseudorange residuals */ nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp, &ns,rtk); if (nv<4) { //sprintf(msg,"lack of valid sats ns=%d",nv); break; } //added by yuan, ̽��ϴ�ֲ���н�ȷ�������������; if ( (rtk->counter>=1) && (i>=1) ){ for (j = 0; j < nv;j++){ if (fabs(v[j]) != 0.0){ rms += v[j]; nv_++; } } rms = rms / nv_; for (j = 0; j < nv_; j++){ RMS += (v[j] - rms)*(v[j] - rms); } RMS = sqrt(RMS/nv_); //�в�ľ�����; if (RMS>2.5){ temp = fabs(v[0] - rms); for (j = 1; j < nv_;j++){ if (temp <= fabs(v[j] - rms)){ temp = fabs(v[j] - rms); kk = j; } else{ continue; } } opt->exsats[rtk->sat_[kk] - 1] = 1; } rms = 0; RMS = 0; nv_ = 0; temp = 0; kk = 0; //nv = nv - 1; //����һ������; } if (nv < NX) { //sprintf(msg, "lack of valid sats ns=%d", nv); break; } /* weight by variance */ for (j = 0; j < nv; j++) { sig = sqrt(var[j]); v[j] /= sig; for (k = 0; k < NX; k++) H[k + j*NX] /= sig; } /* least square estimation */ if ((info=lsq(H,v,NX,nv,dx,Q))) { //sprintf(msg,"lsq error info=%d",info); break; } for (j=0;j<NX;j++) x[j]+=dx[j]; if (norm(dx,NX)<1E-4) { sol->type=0; sol->time=timeadd(obs[0].time,-x[3]/CLIGHT); sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */ sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */ sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */ sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */ for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0; for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX]; sol->qr[3]=(float)Q[1]; /* cov xy */ sol->qr[4]=(float)Q[2+NX]; /* cov yz */ sol->qr[5]=(float)Q[2]; /* cov zx */ sol->ns=(unsigned char)ns; sol->age=sol->ratio=0.0; /* validate solution */ if ((stat = valsol(azel, vsat, n, opt, v, nv, NX, msg))) { sol->stat = opt->sateph == EPHOPT_SBAS ? SOLQ_SBAS : SOLQ_SINGLE; } return stat; } } return 0; }
/* estimate receiver position ------------------------------------------------*/ static int estpos(const obsd_t *obs, int n, const double *rs, const double *dts, const double *vare, const int *svh, const nav_t *nav, const prcopt_t *opt, sol_t *sol, double *azel, int *vsat, double *resp, char *msg) { double x[NX]={0},dx[NX],Q[NX*NX],*v,*H,*var,sig; int i,j,k,info,stat,nv,ns; trace(3,"estpos : n=%d\n",n); v=mat(n+4,1); H=mat(NX,n+4); var=mat(n+4,1); for (i=0;i<3;i++) x[i]=sol->rr[i]; fprintf(output, " Begin coordinates calculation in single point positioning mode\n"); fprintf(output, " Current coordinates: X = %f, Y = %f, Z = %f\n", x[0], x[1], x[2]); for (i=0;i<MAXITR;i++) { fprintf(output, " Begin iteration %i:\n", i); /* pseudorange residuals */ nv=rescode(i,obs,n,rs,dts,vare,svh,nav,x,opt,v,H,var,azel,vsat,resp, &ns); if (nv<NX) { sprintf(msg,"lack of valid sats ns=%d",nv); break; } /* weight by variance */ for (j=0;j<nv;j++) { sig=sqrt(var[j]); v[j]/=sig; for (k=0;k<NX;k++) H[k+j*NX]/=sig; } fprintf(output, " Begin least-square estimation:\n"); fprintf(output, " "); for(j = 0; j < NX; j++) fprintf(output, " X[%i] = %f,", j, dx[j]); fprintf(output, "\n"); fprintf(output, " "); for(j = 0; j < nv; j++) fprintf(output, " v[%i] = %f,", j, v[j]); fprintf(output, "\n"); for(j = 0; j < nv; j++) { fprintf(output, " "); for(k = 0; k < NX; k++) fprintf(output, " H[%i][%i] = %f,", j, k, H[k + j * NX]); fprintf(output, "\n"); } /* least square estimation */ if ((info=lsq(H,v,NX,nv,dx,Q))) { sprintf(msg,"lsq error info=%d",info); break; } fprintf(output, " End least-square estimation\n"); fprintf(output, " Results of least square estimation: dx1 = %f, dx2 = %f, dx3 = %f, dx4 = %f, dx5 = %f, dx6 = %f, dx7 = %f\n", dx[0], dx[1], dx[2], dx[3], dx[4], dx[5], dx[6]); for (j=0;j<NX;j++) x[j]+=dx[j]; fprintf(output, " New state of vector X: X[0] = %f, X[1] = %f, X[2] = %f, X[3] = %f, X[4] = %f, X[5] = %f, X[6] = %f\n", x[0], x[1], x[2], x[3], x[4], x[5], x[6]); fprintf(output, " norm(dx) = %f\n", norm(dx, NX)); if (norm(dx,NX)<1E-4) { sol->type=0; sol->time=timeadd(obs[0].time,-x[3]/CLIGHT); sol->dtr[0]=x[3]/CLIGHT; /* receiver clock bias (s) */ sol->dtr[1]=x[4]/CLIGHT; /* glo-gps time offset (s) */ sol->dtr[2]=x[5]/CLIGHT; /* gal-gps time offset (s) */ sol->dtr[3]=x[6]/CLIGHT; /* bds-gps time offset (s) */ for (j=0;j<6;j++) sol->rr[j]=j<3?x[j]:0.0; for (j=0;j<3;j++) sol->qr[j]=(float)Q[j+j*NX]; sol->qr[3]=(float)Q[1]; /* cov xy */ sol->qr[4]=(float)Q[2+NX]; /* cov yz */ sol->qr[5]=(float)Q[2]; /* cov zx */ sol->ns=(unsigned char)ns; sol->age=sol->ratio=0.0; /* validate solution */ if ((stat=valsol(azel,vsat,n,opt,v,nv,NX,msg))) { sol->stat=opt->sateph==EPHOPT_SBAS?SOLQ_SBAS:SOLQ_SINGLE; } free(v); free(H); free(var); fprintf(output, " End of coordinates calculation\n"); return stat; } } if (i>=MAXITR) sprintf(msg,"iteration divergent i=%d",i); free(v); free(H); free(var); return 0; }