/* decode download file ------------------------------------------------------*/ static void decodefile(rtksvr_t *svr, int index) { nav_t nav; char file[1024]; int nb; INIT_ZERO(nav); tracet(4,"decodefile: index=%d\n",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') { rtksvrunlock(svr); return; } 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 */ /* read sp3 precise ephemeris */ readsp3(file,&nav,0); if (nav.ne<=0) { tracet(1,"sp3 file read error: %s\n",file); return; } /* update precise ephemeris */ 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()); strcpy(svr->files[index],file); rtksvrunlock(svr); } else if (svr->format[index]==STRFMT_RNXCLK) { /* precise clock */ /* read rinex clock */ if (readrnxc(file,&nav)<=0) { tracet(1,"rinex clock file read error: %s\n",file); return; } /* update precise clock */ 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()); strcpy(svr->files[index],file); rtksvrunlock(svr); } }
/* decode binex mesaage 0x01-02: decoded glonass ephmemeris ------------------*/ static int decode_bnx_01_02(raw_t *raw, unsigned char *buff, int len) { geph_t geph={0}; unsigned char *p=buff; double tod,tof,tau_gps; int prn,day,leap; trace(4,"binex 0x01-02: len=%d\n",len); if (len>=119) { prn =U1(p)+1; p+=1; day =U2(p); p+=2; tod =U4(p); p+=4; geph.taun =-R8(p); p+=8; geph.gamn =R8(p); p+=8; tof =U4(p); p+=4; geph.pos[0]=R8(p)*1E3; p+=8; geph.vel[0]=R8(p)*1E3; p+=8; geph.acc[0]=R8(p)*1E3; p+=8; geph.pos[1]=R8(p)*1E3; p+=8; geph.vel[1]=R8(p)*1E3; p+=8; geph.acc[1]=R8(p)*1E3; p+=8; geph.pos[2]=R8(p)*1E3; p+=8; geph.vel[2]=R8(p)*1E3; p+=8; geph.acc[2]=R8(p)*1E3; p+=8; geph.svh =U1(p)&0x1; p+=1; geph.frq =I1(p); p+=1; geph.age =U1(p); p+=1; leap =U1(p); p+=1; tau_gps =R8(p); p+=8; geph.dtaun =R8(p); } else { trace(2,"binex 0x01-02: length error len=%d\n",len); return -1; } if (!(geph.sat=satno(SYS_GLO,prn))) { trace(2,"binex 0x01-02: satellite error prn=%d\n",prn); return -1; } if (raw->time.time==0) return 0; geph.toe=utc2gpst(adjday(raw->time,tod-10800.0)); geph.tof=utc2gpst(adjday(raw->time,tof-10800.0)); geph.iode=(int)(fmod(tod+10800.0,86400.0)/900.0+0.5); if (!strstr(raw->opt,"-EPHALL")) { if (fabs(timediff(geph.toe,raw->nav.geph[prn-MINPRNGLO].toe))<1.0&& geph.svh==raw->nav.geph[prn-MINPRNGLO].svh) return 0; /* unchanged */ } raw->nav.geph[prn-1]=geph; raw->ephsat=geph.sat; return 2; }
/* decode solution time ------------------------------------------------------*/ static char *decode_soltime(char *buff, const solopt_t *opt, gtime_t *time) { double v[MAXFIELD]; char *p,*q,s[64]=" "; int n,len; trace(4,"decode_soltime:\n"); if (!strcmp(opt->sep,"\\t")) strcpy(s,"\t"); else if (*opt->sep) strcpy(s,opt->sep); 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 (v[0]<100.0) { v[0]+=v[0]<80.0?2000.0:1900.0; } *time=epoch2time(v); if (opt->times==TIMES_UTC) { *time=utc2gpst(*time); } else if (opt->times==TIMES_JST) { *time=utc2gpst(timeadd(*time,-9*3600.0)); } 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 (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++; return p+len; } /* wwww ssss */ 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) break; } 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]); return p; } return NULL; }
/* decode solution -----------------------------------------------------------*/ static int decode_gw10sol(raw_t *raw) { gtime_t time; double ep[6]={0},sec; unsigned char *p=raw->buff+6; trace(4,"decode_gw10sol : len=%d\n",raw->len); if (U2(p+42)&0xC00) { /* time valid? */ trace(2,"gw10 sol time/day invalid\n"); return 0; } sec=U4(p+27)/16384.0; sec=floor(sec*1000.0+0.5)/1000.0; ep[2]=bcd2num(p[31]); ep[1]=bcd2num(p[32]); ep[0]=bcd2num(p[33])*100+bcd2num(p[34]); time=utc2gpst(timeadd(epoch2time(ep),sec)); /* set time if no time available */ if (raw->time.time==0) { raw->time=time; } return 0; }
/* tle_pos() -----------------------------------------------------------------*/ static void utest2(void) { const char *file2="../data/tle/tle_sgp4.txt"; const double ep0[6]={1980,1,1}; tle_t tle={0}; gtime_t epoch; double min,rs[6]; int i,stat; epoch=utc2gpst(timeadd(epoch2time(ep0),274.98708465*86400.0)); stat=tle_read(file2,&tle); assert(stat); stat=tle_pos(epoch,"TEST_ERR","","",&tle,NULL,rs); assert(!stat); for (i=0;i<5;i++) { min=360.0*i; stat=tle_pos(timeadd(epoch,min*60.0),"TEST_SAT","","",&tle,NULL,rs); assert(stat); fprintf(OUT,"%4.0f: %14.8f %14.8f %14.8f %11.8f %11.8f %11.8f\n",min, rs[0]/1e3,rs[1]/1e3,rs[2]/1e3,rs[3]/1e3,rs[4]/1e3,rs[5]/1e3); } fprintf(OUT,"%s utest2 : OK\n",__FILE__); }
/* convert glonass time to time ----------------------------------------------- * convert glonass time in ephemeris to gtime_t struct * args : int nt I current day * int n4 I 4 year interval * int h I hour * int m I minute * int s I second * return : gtime_t struct * note : Reference: GLONASS ICD A.3.1.3 *-----------------------------------------------------------------------------*/ gtime_t glot2time(int nt, int n4, int h, int m, int s) { double ep[6]; int j=0,year,mon,day=0,doy=0; int doys[]= {1,32,60,91,121,152,182,213,244,274,305,335}; /* day of year */ int doysl[]={1,32,61,92,122,153,183,214,245,275,306,336}; /* leap year */ ep[3]=h; ep[4]=m; ep[5]=s; if (nt<= 366) {j=1; doy=nt;} else if(nt<= 731) {j=2; doy=nt-366+1;} else if(nt<=1096) {j=3; doy=nt-731+1;} else if(nt<=1461) {j=4; doy=nt-1096+1;} /* current year */ year=1996+4*(n4-1)+(j-1); /* compute current month and day */ for (mon=1;mon<12;mon++) { if (j==1) { if (doy<doysl[mon]) { day=doy-doysl[mon-1]; break; } } else { if (doy<doys[ mon]) { day=doy-doys[mon-1]; break; } } } ep[0]=year; ep[1]=mon; ep[2]=day; return utc2gpst(epoch2time(ep)); /* convert to gtime_t */ }
/* main ----------------------------------------------------------------------*/ int main(int argc, char **argv) { gtime_t time; double ep[6]= {0}; int i,page=1; time2epoch(utc2gpst(timeget()),ep); ep[4]=ep[5]=0.0; for (i=1; i<argc; i++) { if (!strcmp(argv[i],"-t")&&i+2<argc) { sscanf(argv[++i],"%lf/%lf/%lf",ep ,ep+1,ep+2); sscanf(argv[++i],"%lf:%lf:%lf",ep+3,ep+4,ep+5); } else if (!strcmp(argv[i],"-p")&&i+1<argc) page=atoi(argv[++i]); } time=epoch2time(ep); switch (page) { case 1: gen_page1(time,TEMP1,PAGE1); break; case 2: gen_page2(time,TEMP2,PAGE2); break; case 3: gen_page3(time,TEMP3,PAGE3); break; } return 0; }
/* decode nmea gpgga: fix information ----------------------------------------*/ static 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; trace(4,"decode_nmeagga: n=%d\n",n); for (i=0;i<n;i++) { switch (i) { case 0: tod =atof(val[i]); break; /* time in utc (hhmmss) */ case 1: lat =atof(val[i]); break; /* latitude (ddmm.mmm) */ case 2: ns =*val[i]; break; /* N=north,S=south */ case 3: lon =atof(val[i]); break; /* longitude (dddmm.mmm) */ case 4: ew =*val[i]; break; /* E=east,W=west */ case 5: solq=atoi(val[i]); break; /* fix quality */ case 6: nrcv=atoi(val[i]); break; /* # of satellite tracked */ case 7: hdop=atof(val[i]); break; /* hdop */ case 8: alt =atof(val[i]); break; /* altitude in msl */ case 9: ua =*val[i]; break; /* unit (M) */ case 10: msl =atof(val[i]); break; /* height of geoid */ case 11: um =*val[i]; break; /* unit (M) */ } } if ((ns!='N'&&ns!='S')||(ew!='E'&&ew!='W')) { trace(2,"invalid nmea gpgga format\n"); return 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; 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; pos2ecef(pos,sol->rr); sol->stat=0<=solq&&solq<=8?solq_nmea[solq]:SOLQ_NONE; sol->ns=nrcv; 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, hdop,ua,um); return 1; }
//--------------------------------------------------------------------------- void __fastcall TStartDialog::FormShow(TObject *Sender) { char tstr[64]; if (Time.time==0) { Time=utc2gpst(timeget()); } time2str(Time,tstr,0); tstr[10]='\0'; TimeY1->Text=tstr; TimeH1->Text=tstr+11; }
/* decode type 18: rtk uncorrected carrier-phase -----------------------------*/ static int decode_type18(rtcm_t *rtcm) { gtime_t time; double usec,cp,tt; int i=48,index,freq,sync=1,code,sys,prn,sat,loss; rtklib_trace(4,"decode_type18: 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 18 length error: len=%d\n",rtcm->len); return -1; } if (freq&0x1) { rtklib_trace(2,"rtcm2 18 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+3; loss=getbitu(rtcm->buff,i, 5); i+= 5; cp =getbits(rtcm->buff,i,32); i+=32; if (prn==0) prn=32; if (!(sat=satno(sys?SYS_GLO:SYS_GPS,prn))) { rtklib_trace(2,"rtcm2 18 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].L[freq]=-cp/256.0; rtcm->obs.data[index].LLI[freq]=rtcm->loss[sat-1][freq]!=loss; rtcm->obs.data[index].code[freq]= !freq?(code?CODE_L1P:CODE_L1C):(code?CODE_L2P:CODE_L2C); rtcm->loss[sat-1][freq]=loss; } } rtcm->obsflag=!sync; return sync?0:1; }
/* merge ephmeris data --------------------------------------------------------- * merge separated ephmeris data to ephemeris struct * args : sdreph_t *eph I/O sdr ephemeris structure * return : none *-----------------------------------------------------------------------------*/ void merge_g1(sdreph_t *eph) { double ep[6]; eph->geph.tof=glot2time(eph->nt,eph->n4,eph->tk[0],eph->tk[1],eph->tk[2]); /* compute current tow (tk is time of word 1) */ eph->tow_gpst=time2gpst(eph->geph.tof,&eph->eph.week)+eph->s1cnt*2.0; eph->week_gpst=eph->eph.week; ep[3]=0;ep[4]=eph->geph.iode-60*3;ep[5]=0; /* 3 hour bias in UTC-GPST */ eph->geph.toe=utc2gpst(epoch2time(ep)); /* time of ephemeris */ }
/* send nmea request ----------------------------------------------------------- * send nmea gpgga message to stream * args : stream_t *stream I stream * double *pos I position {x,y,z} (ecef) (m) * return : none *-----------------------------------------------------------------------------*/ extern void strsendnmea(stream_t *stream, const double *pos) { sol_t sol={{0}}; unsigned char buff[1024]; int i,n; tracet(3,"strsendnmea: pos=%.3f %.3f %.3f\n",pos[0],pos[1],pos[2]); sol.stat=SOLQ_SINGLE; sol.time=utc2gpst(timeget()); for (i=0;i<3;i++) sol.rr[i]=pos[i]; n=outnmea_gga(buff,&sol); strwrite(stream,buff,n); }
/* adjust hourly rollover of rtcm 2 time -------------------------------------*/ static void adjhour(rtcm_t *rtcm, double zcnt) { double tow,hour,sec; int week; /* if no time, get cpu time */ if (rtcm->time.time==0) rtcm->time=utc2gpst(timeget()); tow=time2gpst(rtcm->time,&week); hour=floor(tow/3600.0); sec=tow-hour*3600.0; if (zcnt<sec-1800.0) zcnt+=3600.0; else if (zcnt>sec+1800.0) zcnt-=3600.0; rtcm->time=gpst2time(week,hour*3600+zcnt); }
/* utc2gpst(), gpst2utc() */ void utest8(void) { double ep0[]={1980, 1, 6, 0, 0, 0.000000}; double ep1[]={2010,12,31,23,59,59.999999}; gtime_t t0,t1,t2,t3; t0=epoch2time(ep0); t1=epoch2time(ep1); while (t0.time<t1.time) { t2=utc2gpst(t0); t3=gpst2utc(t2); assert(t0.time==t3.time&&t0.sec==t3.sec); t0.time+=86400.0; } printf("%s utset8 : OK\n",__FILE__); }
// 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); }
/* decode nmea gprmc: recommended minumum data for gps -----------------------*/ static 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'; int i; trace(4,"decode_nmearmc: n=%d\n",n); for (i=0;i<n;i++) { switch (i) { 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')) { 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; 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)); pos2ecef(pos,sol->rr); sol->stat=mode=='D'?SOLQ_DGPS:SOLQ_SINGLE; sol->ns=0; 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, vel,dir,ang,mew,mode); return 1; }
/* decode id#20 navigation data (user) ---------------------------------------*/ static int decode_ss2llh(raw_t *raw) { double ep[6]; unsigned char *p=raw->buff+4; trace(4,"decode_ss2llh: len=%d\n",raw->len); if (raw->len!=77) { trace(2,"ss2 id#20 length error: len=%d\n",raw->len); return -1; } ep[3]=U1(p ); ep[4]=U1(p+ 1); ep[5]=R8(p+ 2); ep[2]=U1(p+10); ep[1]=U1(p+11); ep[0]=U2(p+12); raw->time=utc2gpst(epoch2time(ep)); return 0; }
static jboolean open_solution_status_file(JNIEnv* env, int level, gtime_t timestamp) { double ep[6]; char filename[1024]; if (level <= 0) return JNI_FALSE; time2epoch(utc2gpst(timestamp),ep); sprintf(filename,"rtkgps_%04.0f%02.0f%02.0f%02.0f%02.0f%02.0f.stat", ep[0],ep[1],ep[2],ep[3],ep[4],ep[5]); if (!get_path_in_storage_dir(env, filename, sizeof(filename))) return JNI_FALSE; LOGV("open_solution_status_file() %s", filename); rtkopenstat(filename, level); return JNI_TRUE; }
/* str2str -------------------------------------------------------------------*/ int main(int argc, char **argv) { static char cmd[MAXRCVCMD]=""; const char ss[]={'E','-','W','C','C'}; strconv_t *conv[MAXSTR]={NULL}; double pos[3],stapos[3]={0}; char *paths[MAXSTR],s[MAXSTR][MAXSTRPATH]={{0}},*cmdfile=""; char *local="",*proxy="",*msg="1004,1019",*opt="",buff[256],*p; char strmsg[MAXSTRMSG]=""; int i,n=0,dispint=5000,trlevel=0,opts[]={10000,10000,2000,32768,10,0,30}; int types[MAXSTR]={0},stat[MAXSTR]={0},byte[MAXSTR]={0},bps[MAXSTR]={0}; int fmts[MAXSTR],sta=0; for (i=0;i<MAXSTR;i++) paths[i]=s[i]; for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-in")&&i+1<argc) { if (!decodepath(argv[++i],types,paths[0],fmts)) return -1; } else if (!strcmp(argv[i],"-out")&&i+1<argc&&n<MAXSTR-1) { if (!decodepath(argv[++i],types+n+1,paths[n+1],fmts+n+1)) return -1; n++; } else if (!strcmp(argv[i],"-p")&&i+3<argc) { pos[0]=atof(argv[++i])*D2R; pos[1]=atof(argv[++i])*D2R; pos[2]=atof(argv[++i]); pos2ecef(pos,stapos); } else if (!strcmp(argv[i],"-msg")&&i+1<argc) msg=argv[++i]; else if (!strcmp(argv[i],"-opt")&&i+1<argc) opt=argv[++i]; else if (!strcmp(argv[i],"-sta")&&i+1<argc) sta=atoi(argv[++i]); else if (!strcmp(argv[i],"-d" )&&i+1<argc) dispint=atoi(argv[++i]); else if (!strcmp(argv[i],"-s" )&&i+1<argc) opts[0]=atoi(argv[++i]); else if (!strcmp(argv[i],"-r" )&&i+1<argc) opts[1]=atoi(argv[++i]); else if (!strcmp(argv[i],"-n" )&&i+1<argc) opts[5]=atoi(argv[++i]); else if (!strcmp(argv[i],"-f" )&&i+1<argc) opts[6]=atoi(argv[++i]); else if (!strcmp(argv[i],"-c" )&&i+1<argc) cmdfile=argv[++i]; else if (!strcmp(argv[i],"-l" )&&i+1<argc) local=argv[++i]; else if (!strcmp(argv[i],"-x" )&&i+1<argc) proxy=argv[++i]; else if (!strcmp(argv[i],"-t" )&&i+1<argc) trlevel=atoi(argv[++i]); else if (*argv[i]=='-') printhelp(); } if (!*paths[0]) { fprintf(stderr,"specify input stream\n"); return -1; } if (n<=0) { fprintf(stderr,"specify output stream(s)\n"); return -1; } for (i=0;i<n;i++) { if (fmts[i+1]<0) continue; if (fmts[i+1]!=STRFMT_RTCM3) { fprintf(stderr,"unsupported output format\n"); return -1; } if (fmts[0]<0) { fprintf(stderr,"specify input format\n"); return -1; } if (!(conv[i]=strconvnew(fmts[0],fmts[i+1],msg,sta,sta!=0,opt))) { fprintf(stderr,"stream conversion error\n"); return -1; } } signal(SIGTERM,sigfunc); signal(SIGINT ,sigfunc); signal(SIGHUP ,SIG_IGN); signal(SIGPIPE,SIG_IGN); strsvrinit(&strsvr,n+1); if (trlevel>0) { traceopen(TRFILE); tracelevel(trlevel); } fprintf(stderr,"stream server start\n"); strsetdir(local); strsetproxy(proxy); if (*cmdfile) readcmd(cmdfile,cmd,0); /* start stream server */ if (!strsvrstart(&strsvr,opts,types,paths,conv,*cmd?cmd:NULL,stapos)) { fprintf(stderr,"stream server start error\n"); return -1; } for (intrflg=0;!intrflg;) { /* get stream server status */ strsvrstat(&strsvr,stat,byte,bps,strmsg); /* show stream server status */ for (i=0,p=buff;i<MAXSTR;i++) p+=sprintf(p,"%c",ss[stat[i]+1]); fprintf(stderr,"%s [%s] %10d B %7d bps %s\n", time_str(utc2gpst(timeget()),0),buff,byte[0],bps[0],strmsg); sleepms(dispint); } if (*cmdfile) readcmd(cmdfile,cmd,1); /* stop stream server */ strsvrstop(&strsvr,*cmd?cmd:NULL); for (i=0;i<n;i++) { strconvfree(conv[i]); } if (trlevel>0) { traceclose(); } fprintf(stderr,"stream server stop\n"); return 0; }
//--------------------------------------------------------------------------- void __fastcall TOptDialog::SaveOpt(AnsiString file) { AnsiString ProxyAddrE_Text=ProxyAddrE->Text; AnsiString ExSatsE_Text=ExSatsE->Text; AnsiString FieldSep_Text=FieldSep->Text; AnsiString RovAnt_Text=RovAnt->Text,RefAnt_Text=RefAnt->Text; AnsiString SatPcvFile_Text=SatPcvFile->Text; AnsiString AntPcvFile_Text=AntPcvFile->Text; AnsiString StaPosFile_Text=StaPosFile->Text; AnsiString GeoidDataFile_Text=GeoidDataFile->Text; AnsiString DCBFile_Text=DCBFile->Text; AnsiString LocalDir_Text=LocalDir->Text; int itype[]={STR_SERIAL,STR_TCPCLI,STR_TCPSVR,STR_NTRIPCLI,STR_FILE,STR_FTP,STR_HTTP}; int otype[]={STR_SERIAL,STR_TCPCLI,STR_TCPSVR,STR_NTRIPSVR,STR_FILE}; TEdit *editu[]={RovPos1,RovPos2,RovPos3}; TEdit *editr[]={RefPos1,RefPos2,RefPos3}; char buff[1024],*p,id[32],comment[256],s[64]; int sat,ex; prcopt_t prcopt=prcopt_default; solopt_t solopt=solopt_default; filopt_t filopt={""}; for (int i=0;i<8;i++) { strtype[i]=i<3?itype[MainForm->Stream[i]]:otype[MainForm->Stream[i]]; strfmt[i]=MainForm->Format[i]; if (!MainForm->StreamC[i]) { strtype[i]=STR_NONE; strcpy(strpath[i],""); } else if (strtype[i]==STR_SERIAL) { strcpy(strpath[i],MainForm->Paths[i][0].c_str()); } else if (strtype[i]==STR_FILE) { strcpy(strpath[i],MainForm->Paths[i][2].c_str()); } else if (strtype[i]<=STR_NTRIPCLI) { strcpy(strpath[i],MainForm->Paths[i][1].c_str()); } else if (strtype[i]<=STR_HTTP) { strcpy(strpath[i],MainForm->Paths[i][3].c_str()); } } nmeareq =MainForm->NmeaReq; nmeapos[0]=MainForm->NmeaPos[0]; nmeapos[1]=MainForm->NmeaPos[1]; svrcycle =SvrCycleE ->Text.ToInt(); timeout =TimeoutTimeE->Text.ToInt(); reconnect =ReconTimeE ->Text.ToInt(); nmeacycle =NmeaCycleE ->Text.ToInt(); buffsize =SvrBuffSizeE->Text.ToInt(); navmsgsel =NavSelectS ->ItemIndex; strcpy(proxyaddr,ProxyAddrE_Text.c_str()); fswapmargin =FileSwapMarginE->Text.ToInt(); prcopt.sbassatsel=SbasSatE->Text.ToInt(); prcopt.mode =PosMode ->ItemIndex; prcopt.nf =Freq ->ItemIndex+1; prcopt.soltype =Solution ->ItemIndex; prcopt.elmin =str2dbl(ElMask ->Text)*D2R; prcopt.dynamics =DynamicModel->ItemIndex; prcopt.tidecorr =TideCorr ->ItemIndex; prcopt.ionoopt =IonoOpt ->ItemIndex; prcopt.tropopt =TropOpt ->ItemIndex; prcopt.sateph =SatEphem ->ItemIndex; if (ExSatsE->Text!="") { strcpy(buff,ExSatsE_Text.c_str()); for (p=strtok(buff," ");p;p=strtok(NULL," ")) { if (*p=='+') {ex=2; p++;} else ex=1; if (!(sat=satid2no(p))) continue; prcopt.exsats[sat-1]=ex; } } prcopt.navsys = (NavSys1->Checked?SYS_GPS:0)| (NavSys2->Checked?SYS_GLO:0)| (NavSys3->Checked?SYS_GAL:0)| (NavSys4->Checked?SYS_QZS:0)| (NavSys5->Checked?SYS_SBS:0)| (NavSys6->Checked?SYS_CMP:0); prcopt.posopt[0]=PosOpt1->Checked; prcopt.posopt[1]=PosOpt2->Checked; prcopt.posopt[2]=PosOpt3->Checked; prcopt.posopt[3]=PosOpt4->Checked; prcopt.posopt[4]=PosOpt5->Checked; prcopt.modear =AmbRes ->ItemIndex; prcopt.glomodear=GloAmbRes ->ItemIndex; prcopt.thresar[0]=str2dbl(ValidThresAR->Text); prcopt.maxout =str2dbl(OutCntResetAmb->Text); prcopt.minfix =str2dbl(FixCntHoldAmb->Text); prcopt.minlock =str2dbl(LockCntFixAmb->Text); prcopt.elmaskar =str2dbl(ElMaskAR ->Text)*D2R; prcopt.elmaskhold=str2dbl(ElMaskHold->Text)*D2R; prcopt.maxtdiff =str2dbl(MaxAgeDiff ->Text); prcopt.maxgdop =str2dbl(RejectGdop ->Text); prcopt.maxinno =str2dbl(RejectThres->Text); prcopt.thresslip=str2dbl(SlipThres ->Text); prcopt.niter =str2dbl(NumIter ->Text); prcopt.syncsol =SyncSol->ItemIndex; if (prcopt.mode==PMODE_MOVEB&&BaselineConst->Checked) { prcopt.baseline[0]=str2dbl(BaselineLen->Text); prcopt.baseline[1]=str2dbl(BaselineSig->Text); } solopt.posf =SolFormat ->ItemIndex; solopt.timef =TimeFormat ->ItemIndex==0?0:1; solopt.times =TimeFormat ->ItemIndex==0?0:TimeFormat->ItemIndex-1; solopt.timeu =str2dbl(TimeDecimal ->Text); solopt.degf =LatLonFormat->ItemIndex; strcpy(solopt.sep,FieldSep_Text.c_str()); solopt.outhead =OutputHead ->ItemIndex; solopt.outopt =OutputOpt ->ItemIndex; solopt.datum =OutputDatum ->ItemIndex; solopt.height =OutputHeight->ItemIndex; solopt.geoid =OutputGeoid ->ItemIndex; solopt.nmeaintv[0]=str2dbl(NmeaIntv1->Text); solopt.nmeaintv[1]=str2dbl(NmeaIntv2->Text); solopt.trace =DebugTrace ->ItemIndex; solopt.sstat =DebugStatus ->ItemIndex; prcopt.eratio[0]=str2dbl(MeasErrR1->Text); prcopt.eratio[1]=str2dbl(MeasErrR2->Text); prcopt.err[1] =str2dbl(MeasErr2->Text); prcopt.err[2] =str2dbl(MeasErr3->Text); prcopt.err[3] =str2dbl(MeasErr4->Text); prcopt.err[4] =str2dbl(MeasErr5->Text); prcopt.sclkstab =str2dbl(SatClkStab->Text); prcopt.prn[0] =str2dbl(PrNoise1->Text); prcopt.prn[1] =str2dbl(PrNoise2->Text); prcopt.prn[2] =str2dbl(PrNoise3->Text); prcopt.prn[3] =str2dbl(PrNoise4->Text); prcopt.prn[4] =str2dbl(PrNoise5->Text); if (RovAntPcv->Checked) strcpy(prcopt.anttype[0],RovAnt_Text.c_str()); if (RefAntPcv->Checked) strcpy(prcopt.anttype[1],RefAnt_Text.c_str()); prcopt.antdel[0][0]=str2dbl(RovAntE->Text); prcopt.antdel[0][1]=str2dbl(RovAntN->Text); prcopt.antdel[0][2]=str2dbl(RovAntU->Text); prcopt.antdel[1][0]=str2dbl(RefAntE->Text); prcopt.antdel[1][1]=str2dbl(RefAntN->Text); prcopt.antdel[1][2]=str2dbl(RefAntU->Text); prcopt.rovpos=RovPosTypeP->ItemIndex<3?0:4; prcopt.refpos=RefPosTypeP->ItemIndex<3?0:4; if (prcopt.rovpos==0) GetPos(RovPosTypeP->ItemIndex,editu,prcopt.ru); if (prcopt.refpos==0) GetPos(RefPosTypeP->ItemIndex,editr,prcopt.rb); strcpy(filopt.satantp,SatPcvFile_Text.c_str()); strcpy(filopt.rcvantp,AntPcvFile_Text.c_str()); strcpy(filopt.stapos, StaPosFile_Text.c_str()); strcpy(filopt.geoid, GeoidDataFile_Text.c_str()); strcpy(filopt.dcb, DCBFile_Text.c_str()); strcpy(filopt.tempdir,LocalDir_Text.c_str()); time2str(utc2gpst(timeget()),s,0); sprintf(comment,"RTKNAVI options (%s, v.%s)",s,VER_RTKLIB); setsysopts(&prcopt,&solopt,&filopt); if (!saveopts(file.c_str(),"w",comment,sysopts)|| !saveopts(file.c_str(),"a","",rcvopts)) return; }
/* str2str -------------------------------------------------------------------*/ int main(int argc, char **argv) { static char cmd[MAXRCVCMD]=""; const char ss[]={'E','-','W','C','C'}; strconv_t *conv[MAXSTR]={NULL}; double pos[3],stapos[3]={0},off[3]={0}; char *paths[MAXSTR],s[MAXSTR][MAXSTRPATH]={{0}},*cmdfile=""; char *local="",*proxy="",*msg="1004,1019",*opt="",buff[256],*p; char strmsg[MAXSTRMSG]="",*antinfo="",*rcvinfo=""; char *ant[]={"","",""},*rcv[]={"","",""}; int i,j,n=0,dispint=5000,trlevel=0,opts[]={10000,10000,2000,32768,10,0,30}; int types[MAXSTR]={STR_FILE,STR_FILE},stat[MAXSTR]={0},byte[MAXSTR]={0}; int bps[MAXSTR]={0},fmts[MAXSTR]={0},sta=0; int ctl_port=2097; char ctl_buffer[256]; char *nodefile; int sockfd; char *rip; struct sockaddr_in serv_addr; rip=ctl_buffer; fd_set sockset; roverMsg rmsg; for (i=0;i<MAXSTR;i++) paths[i]=s[i]; for (i=1;i<argc;i++) { if (!strcmp(argv[i],"-in")&&i+1<argc) { if (!decodepath(argv[++i],types,paths[0],fmts)) return -1; } else if (!strcmp(argv[i],"-out")&&i+1<argc&&n<MAXSTR-1) { if (!decodepath(argv[++i],types+n+1,paths[n+1],fmts+n+1)) return -1; n++; } else if (!strcmp(argv[i],"-p")&&i+3<argc) { pos[0]=atof(argv[++i])*D2R; pos[1]=atof(argv[++i])*D2R; pos[2]=atof(argv[++i]); pos2ecef(pos,stapos); } else if (!strcmp(argv[i],"-o")&&i+3<argc) { off[0]=atof(argv[++i]); off[1]=atof(argv[++i]); off[2]=atof(argv[++i]); } else if (!strcmp(argv[i],"-msg")&&i+1<argc) msg=argv[++i]; else if (!strcmp(argv[i],"-opt")&&i+1<argc) opt=argv[++i]; else if (!strcmp(argv[i],"-sta")&&i+1<argc) sta=atoi(argv[++i]); else if (!strcmp(argv[i],"-d" )&&i+1<argc) dispint=atoi(argv[++i]); else if (!strcmp(argv[i],"-s" )&&i+1<argc) opts[0]=atoi(argv[++i]); else if (!strcmp(argv[i],"-r" )&&i+1<argc) opts[1]=atoi(argv[++i]); else if (!strcmp(argv[i],"-n" )&&i+1<argc) opts[5]=atoi(argv[++i]); else if (!strcmp(argv[i],"-f" )&&i+1<argc) opts[6]=atoi(argv[++i]); else if (!strcmp(argv[i],"-c" )&&i+1<argc) cmdfile=argv[++i]; else if (!strcmp(argv[i],"-a" )&&i+1<argc) antinfo=argv[++i]; else if (!strcmp(argv[i],"-i" )&&i+1<argc) rcvinfo=argv[++i]; else if (!strcmp(argv[i],"-l" )&&i+1<argc) local=argv[++i]; else if (!strcmp(argv[i],"-x" )&&i+1<argc) proxy=argv[++i]; else if (!strcmp(argv[i],"-t" )&&i+1<argc) trlevel=atoi(argv[++i]); else if (!strcmp(argv[i],"-port")&&i+1<argc) ctl_port=atoi(argv[++i]); else if (!strcmp(argv[i], "-nodefile")&& i+1<argc) nodefile=argv[++i]; else if (!strcmp(argv[i], "-rip")&&i+1<argc) rip=argv[++i]; else if (*argv[i]=='-') printhelp(); } if (n<=0) n=1; /* stdout */ fprintf(stderr,"argv parsed\n"); if(rip==ctl_buffer) { fprintf(stderr, "No rover ip address\n"); return(-1); } serv_addr.sin_family=AF_INET; inet_aton(rip, &serv_addr.sin_addr); serv_addr.sin_port=htons(ctl_port); sockfd=socket(AF_INET,SOCK_STREAM, 0); if (sockfd < 0) fprintf(stderr,"ERROR opening socket\n"); FD_ZERO(&sockset); FD_SET(sockfd, &sockset); for (i=0;i<n;i++) { if (fmts[i+1]<=0) continue; if (fmts[i+1]!=STRFMT_RTCM3) { fprintf(stderr,"unsupported output format\n"); return -1; } if (fmts[0]<0) { fprintf(stderr,"specify input format\n"); return -1; } if (!(conv[i]=strconvnew(fmts[0],fmts[i+1],msg,sta,sta!=0,opt))) { fprintf(stderr,"stream conversion error\n"); return -1; } strcpy(buff,antinfo); for (p=strtok(buff,","),j=0;p&&j<3;p=strtok(NULL,",")) ant[j++]=p; strcpy(conv[i]->out.sta.antdes,ant[0]); strcpy(conv[i]->out.sta.antsno,ant[1]); conv[i]->out.sta.antsetup=atoi(ant[2]); strcpy(buff,rcvinfo); for (p=strtok(buff,","),j=0;p&&j<3;p=strtok(NULL,",")) rcv[j++]=p; strcpy(conv[i]->out.sta.rectype,rcv[0]); strcpy(conv[i]->out.sta.recver ,rcv[1]); strcpy(conv[i]->out.sta.recsno ,rcv[2]); matcpy(conv[i]->out.sta.pos,pos,3,1); matcpy(conv[i]->out.sta.del,off,3,1); } signal(SIGTERM,sigfunc); signal(SIGINT ,sigfunc); signal(SIGHUP ,SIG_IGN); signal(SIGPIPE,SIG_IGN); strsvrinit(&strsvr,n+1); if (trlevel>0) { traceopen(TRFILE); tracelevel(trlevel); } fprintf(stderr,"stream server start\n"); strsetdir(local); strsetproxy(proxy); if (*cmdfile) readcmd(cmdfile,cmd,0); /* start stream server */ if (!strsvrstart(&strsvr,opts,types,paths,conv,*cmd?cmd:NULL,stapos)) { fprintf(stderr,"stream server start error\n"); return -1; } // Socket server start if(connect(sockfd,(struct sockaddr*)&serv_addr,sizeof(serv_addr))<0) fprintf(stderr,"error connecting\n"); if (bind(sockfd, (struct sockaddr *) &serv_addr, sizeof(serv_addr)) < 0) fprintf(stderr,"ERROR on binding"); for (intrflg=0;!intrflg;) { /* get stream server status */ strsvrstat(&strsvr,stat,byte,bps,strmsg); /* show stream server status */ for (i=0,p=buff;i<MAXSTR;i++) p+=sprintf(p,"%c",ss[stat[i]+1]); fprintf(stderr,"%s [%s] %10d B %7d bps %s\n", time_str(utc2gpst(timeget()),0),buff,byte[0],bps[0],strmsg); sleepms(dispint); } if (*cmdfile) readcmd(cmdfile,cmd,1); /* stop stream server */ strsvrstop(&strsvr,*cmd?cmd:NULL); for (i=0;i<n;i++) { strconvfree(conv[i]); } if (trlevel>0) { traceclose(); } fprintf(stderr,"stream server stop\n"); return 0; }
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); }
/* start rtk server ------------------------------------------------------------ * start rtk server thread * args : rtksvr_t *svr IO rtk server * int cycle I server cycle (ms) * int buffsize I input buffer size (bytes) * int *strs I stream types (STR_???) * types[0]=input stream rover * types[1]=input stream base station * types[2]=input stream correction * types[3]=output stream solution 1 * types[4]=output stream solution 2 * types[5]=log stream rover * types[6]=log stream base station * types[7]=log stream correction * char *paths I input stream paths * int *format I input stream formats (STRFMT_???) * format[0]=input stream rover * format[1]=input stream base station * format[2]=input stream correction * int navsel I navigation message select * (0:rover,1:base,2:ephem,3:all) * char **cmds I input stream start commands * cmds[0]=input stream rover (NULL: no command) * cmds[1]=input stream base (NULL: no command) * cmds[2]=input stream corr (NULL: no command) * char **rcvopts I receiver options * rcvopt[0]=receiver option rover * rcvopt[1]=receiver option base * rcvopt[2]=receiver option corr * int nmeacycle I nmea request cycle (ms) (0:no request) * int nmeareq I nmea request type (0:no,1:base pos,2:single sol) * double *nmeapos I transmitted nmea position (ecef) (m) * prcopt_t *prcopt I rtk processing options * solopt_t *solopt I solution options * solopt[0]=solution 1 options * solopt[1]=solution 2 options * stream_t *moni I monitor stream (NULL: not used) * return : status (1:ok 0:error) *-----------------------------------------------------------------------------*/ extern int rtksvrstart(rtksvr_t *svr, int cycle, int buffsize, int *strs, char **paths, int *formats, int navsel, char **cmds, char **rcvopts, int nmeacycle, int nmeareq, const double *nmeapos, prcopt_t *prcopt, solopt_t *solopt, stream_t *moni) { gtime_t time,time0; int i,j,rw; INIT_ZERO(time); INIT_ZERO(time0); tracet(3,"rtksvrstart: cycle=%d buffsize=%d navsel=%d nmeacycle=%d nmeareq=%d\n", cycle,buffsize,navsel,nmeacycle,nmeareq); 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; rtkfree(&svr->rtk); rtkinit(&svr->rtk,prcopt); 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))) { tracet(1,"rtksvrstart: malloc error\n"); return 0; } for (j=0;j<10;j++) svr->nmsg[i][j]=0; for (j=0;j<MAXOBSBUF;j++) svr->obs[i][j].n=0; /* initialize receiver raw and rtcm control */ init_raw (svr->raw +i); init_rtcm(svr->rtcm+i); /* set receiver and rtcm option */ strcpy(svr->raw [i].opt,rcvopts[i]); strcpy(svr->rtcm[i].opt,rcvopts[i]); /* connect dgps corrections */ svr->rtcm[i].dgps=svr->nav.dgps; } for (i=0;i<2;i++) { /* output peek buffer */ 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++) { svr->solopt[i]=solopt[i]; } /* set base station position */ for (i=0;i<6;i++) { svr->rtk.rb[i]=i<3?prcopt->rb[i]:0.0; } /* update navigation data */ for (i=0;i<MAXSAT *2;i++) svr->nav.eph [i].ttr=time0; for (i=0;i<NSATGLO*2;i++) svr->nav.geph[i].tof=time0; for (i=0;i<NSATSBS*2;i++) svr->nav.seph[i].tof=time0; updatenav(&svr->nav); /* set monitor stream */ svr->moni=moni; /* open input streams */ for (i=0;i<8;i++) { 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); 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; } } /* sync input streams */ strsync(svr->stream,svr->stream+1); strsync(svr->stream,svr->stream+2); /* write start commands to input streams */ 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++) { writesolhead(svr->stream+i,svr->solopt+i-3); } /* create rtk server thread */ #ifdef WIN32 if (!(svr->thread=CreateThread(NULL,0,rtksvrthread,svr,0,NULL))) { #else if (pthread_create(&svr->thread,NULL,rtksvrthread,svr)) { #endif for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i); return 0; } return 1; } /* stop rtk server ------------------------------------------------------------- * start rtk server thread * args : rtksvr_t *svr IO rtk server * char **cmds I input stream stop commands * cmds[0]=input stream rover (NULL: no command) * cmds[1]=input stream base (NULL: no command) * cmds[2]=input stream ephem (NULL: no command) * return : none *-----------------------------------------------------------------------------*/ extern void rtksvrstop(rtksvr_t *svr, char **cmds) { int i; tracet(3,"rtksvrstop:\n"); /* write stop commands to input streams */ rtksvrlock(svr); for (i=0;i<3;i++) { if (cmds[i]) strsendcmd(svr->stream+i,cmds[i]); } rtksvrunlock(svr); /* stop rtk server */ svr->state=0; /* free rtk server thread */ #ifdef WIN32 WaitForSingleObject(svr->thread,10000); CloseHandle(svr->thread); #else pthread_join(svr->thread,NULL); #endif }
/* read sp3 body -------------------------------------------------------------*/ static void readsp3b(FILE *fp, char type, int *sats, int ns, double *bfact, char *tsys, int index, int opt, nav_t *nav) { peph_t peph; gtime_t time; double val,std,base; int i,j,sat,sys,prn,n=ns*(type=='P'?1:2),pred_o,pred_c; char buff[1024]; trace(3,"readsp3b: type=%c ns=%d index=%d opt=%d\n",type,ns,index,opt); while (fgets(buff,sizeof(buff),fp)) { if (!strncmp(buff,"EOF",3)) break; if (buff[0]!='*'||str2time(buff,3,28,&time)) { trace(2,"sp3 invalid epoch %31.31s\n",buff); continue; } if (!strcmp(tsys,"UTC")) time=utc2gpst(time); /* utc->gpst */ peph.time =time; peph.index=index; for (i=0;i<MAXSAT;i++) for (j=0;j<4;j++) { peph.pos[i][j]=0.0; peph.std[i][j]=0.0f; } for (i=0;i<n&&fgets(buff,sizeof(buff),fp);i++) { if (strlen(buff)<4||buff[0]!='P') continue; sys=buff[1]==' '?SYS_GPS:code2sys(buff[1]); prn=(int)str2num(buff,2,2); if (sys==SYS_SBS) prn+=100; else if (sys==SYS_QZS) prn+=192; /* extension to sp3-c */ if (!(sat=satno(sys,prn))) continue; pred_c=strlen(buff)>=76&&buff[75]=='P'; pred_o=strlen(buff)>=80&&buff[79]=='P'; for (j=0;j<4;j++) { /* read option for predicted value */ if (j< 3&&opt==1&& pred_o) continue; if (j< 3&&opt==2&&!pred_o) continue; if (j==3&&opt==1&& pred_c) continue; if (j==3&&opt==2&&!pred_c) continue; val=str2num(buff, 4+j*14,14); std=str2num(buff,61+j* 3,j<3?2:3); if (val!=0.0&&fabs(val-999999.999999)>=1E-6) { peph.pos[sat-1][j]=val*(j<3?1000.0:1E-6); } if ((base=bfact[j<3?0:1])>0.0) { peph.std[sat-1][j]=(float)(pow(base,std)*(j<3?1E-3:1E-12)); } } } if (!addpeph(nav,&peph)) return; } }
/* decode gloephem -----------------------------------------------------------*/ static int decode_gloephem(int sat, raw_t *raw) { geph_t geph={0}; unsigned char *p=(raw->buff)+2; int prn,tk,tb; if (raw->len>=93) { prn =I1(p+ 1); geph.frq =I1(p+ 2); geph.pos[0]=R8(p+ 3); geph.pos[1]=R8(p+11); geph.pos[2]=R8(p+19); geph.vel[0]=R8(p+27) * 1e+3; geph.vel[1]=R8(p+35) * 1e+3; geph.vel[2]=R8(p+43) * 1e+3; geph.acc[0]=R8(p+51) * 1e+6; geph.acc[1]=R8(p+59) * 1e+6; geph.acc[2]=R8(p+67) * 1e+6; tb = R8(p+75) * 1e-3; tk = tb; geph.gamn =R4(p+83); geph.taun =R4(p+87) * 1e-3; geph.age =I2(p+91); } else { trace(2,"nvs NE length error: len=%d\n",raw->len); return -1; } if (!(geph.sat=satno(SYS_GLO,prn))) { trace(2,"nvs NE satellite error: prn=%d\n",prn); return -1; } if (raw->time.time==0) return 0; geph.iode=(tb/900)&0x3F; geph.toe=utc2gpst(adjday(raw->time,tb-10800.0)); geph.tof=utc2gpst(adjday(raw->time,tk-10800.0)); #if 0 /* check illegal ephemeris by toe */ tt=timediff(raw->time,geph.toe); if (fabs(tt)>3600.0) { trace(3,"nvs NE illegal toe: prn=%2d tt=%6.0f\n",prn,tt); return 0; } #endif #if 0 /* check illegal ephemeris by frequency number consistency */ if (raw->nav.geph[prn-MINPRNGLO].toe.time&& geph.frq!=raw->nav.geph[prn-MINPRNGLO].frq) { trace(2,"nvs NE illegal freq change: prn=%2d frq=%2d->%2d\n",prn, raw->nav.geph[prn-MINPRNGLO].frq,geph.frq); return -1; } if (!strstr(raw->opt,"-EPHALL")) { if (fabs(timediff(geph.toe,raw->nav.geph[prn-MINPRNGLO].toe))<1.0&& geph.svh==raw->nav.geph[prn-MINPRNGLO].svh) return 0; } #endif raw->nav.geph[prn-1]=geph; raw->ephsat=geph.sat; return 2; }
/* decode glonass ephemeris strings -------------------------------------------- * decode glonass ephemeris string (ref [2]) * args : unsigned char *buff I glonass navigation data string bits in frames * (without hamming and time mark) * buff[ 0- 9]: string #1 (77 bits) * buff[10-19]: string #2 * buff[20-29]: string #3 * buff[30-39]: string #4 * geph_t *geph IO glonass ephemeris message * return : status (1:ok,0:error) * notes : geph->tof should be set to frame time witin 1/2 day before calling * geph->frq is set to 0 *-----------------------------------------------------------------------------*/ extern int decode_glostr(const unsigned char *buff, geph_t *geph) { double tow,tod,tof,toe; int P,P1,P2,P3,P4,tk_h,tk_m,tk_s,tb,ln,NT,slot,M,week; int i=1,frn1,frn2,frn3,frn4; trace(3,"decode_glostr:\n"); /* frame 1 */ frn1 =getbitu(buff,i, 4); i+= 4+2; P1 =getbitu(buff,i, 2); i+= 2; tk_h =getbitu(buff,i, 5); i+= 5; tk_m =getbitu(buff,i, 6); i+= 6; tk_s =getbitu(buff,i, 1)*30; i+= 1; geph->vel[0]=getbitg(buff,i,24)*P2_20*1E3; i+=24; geph->acc[0]=getbitg(buff,i, 5)*P2_30*1E3; i+= 5; geph->pos[0]=getbitg(buff,i,27)*P2_11*1E3; i+=27+4; /* frame 2 */ frn2 =getbitu(buff,i, 4); i+= 4; geph->svh =getbitu(buff,i, 3); i+= 3; P2 =getbitu(buff,i, 1); i+= 1; tb =getbitu(buff,i, 7); i+= 7+5; geph->vel[1]=getbitg(buff,i,24)*P2_20*1E3; i+=24; geph->acc[1]=getbitg(buff,i, 5)*P2_30*1E3; i+= 5; geph->pos[1]=getbitg(buff,i,27)*P2_11*1E3; i+=27+4; /* frame 3 */ frn3 =getbitu(buff,i, 4); i+= 4; P3 =getbitu(buff,i, 1); i+= 1; geph->gamn =getbitg(buff,i,11)*P2_40; i+=11+1; P =getbitu(buff,i, 2); i+= 2; ln =getbitu(buff,i, 1); i+= 1; geph->vel[2]=getbitg(buff,i,24)*P2_20*1E3; i+=24; geph->acc[2]=getbitg(buff,i, 5)*P2_30*1E3; i+= 5; geph->pos[2]=getbitg(buff,i,27)*P2_11*1E3; i+=27+4; /* frame 4 */ frn4 =getbitu(buff,i, 4); i+= 4; geph->taun =getbitg(buff,i,22)*P2_30; i+=22; geph->dtaun =getbitg(buff,i, 5)*P2_30; i+= 5; geph->age =getbitu(buff,i, 5); i+= 5+14; P4 =getbitu(buff,i, 1); i+= 1; geph->sva =getbitu(buff,i, 4); i+= 4+3; NT =getbitu(buff,i,11); i+=11; slot =getbitu(buff,i, 5); i+= 5; M =getbitu(buff,i, 2); if (frn1!=1||frn2!=2||frn3!=3||frn4!=4) { trace(3,"decode_glostr error: frn=%d %d %d %d %d\n",frn1,frn2,frn3,frn4); return 0; } if (!(geph->sat=satno(SYS_GLO,slot))) { trace(2,"decode_glostr error: slot=%d\n",slot); return 0; } geph->frq=0; geph->iode=tb; tow=time2gpst(gpst2utc(geph->tof),&week); tod=fmod(tow,86400.0); tow-=tod; tof=tk_h*3600.0+tk_m*60.0+tk_s-10800.0; /* lt->utc */ if (tof<tod-43200.0) tof+=86400.0; else if (tof>tod+43200.0) tof-=86400.0; geph->tof=utc2gpst(gpst2time(week,tow+tof)); toe=tb*900.0-10800.0; /* lt->utc */ if (toe<tod-43200.0) toe+=86400.0; else if (toe>tod+43200.0) toe-=86400.0; geph->toe=utc2gpst(gpst2time(week,tow+toe)); /* utc->gpst */ return 1; }