/* execute download ------------------------------------------------------------ * execute download * args : gtime_t ts,te I time start and end * double tint I time interval (s) * int seqnos I sequence number start * int seqnoe I sequence number end * url_t *urls I url address list * int nurl I number of urls * char **stas I station list * int nsta I number of stations * char *dir I local directory * char *remote_p I previous remote file path * char *usr I login user for ftp * char *pwd I login password for ftp * char *proxy I proxy server address * int opts I download options (or of the followings) * DLOPT_FORCE = force download existing file * DLOPT_KEEPCMP=keep compressed file * DLOPT_HOLDERR=hold on error file * DLOPT_HOLDLST=hold on listing file * char *msg O output messages * FILE *fp IO log file pointer (NULL: no output log) * return : status (1:ok,0:error,-1:aborted) * notes : urls should be read by using dl_readurl() *-----------------------------------------------------------------------------*/ extern int dl_exec(gtime_t ts, gtime_t te, double ti, int seqnos, int seqnoe, const url_t *urls, int nurl, char **stas, int nsta, const char *dir, const char *usr, const char *pwd, const char *proxy, int opts, char *msg, FILE *fp) { paths_t paths={0}; gtime_t ts_p={0}; char str[2048],remot_p[1024]=""; int i,n[4]={0}; unsigned int tick=tickget(); showmsg("STAT=_"); /* generate download paths */ while (timediff(ts,te)<1E-3) { for (i=0;i<nurl;i++) { if (!gen_paths(ts,ts_p,seqnos,seqnoe,urls+i,stas,nsta,dir,&paths)) { free_path(&paths); return 0; } } ts_p=ts; ts=timeadd(ts,ti); } /* compact download paths */ compact_paths(&paths); if (paths.n<=0) { sprintf(msg,"no download data"); return 0; } for (i=0;i<paths.n;i++) { sprintf(str,"%s->%s (%d/%d)",paths.path[i].remot,paths.path[i].local,i+1, paths.n); if (showmsg(str)) break; /* execute download */ if (exec_down(paths.path+i,remot_p,usr,pwd,proxy,opts,n,fp)) { break; } } if (!(opts&DLOPT_HOLDLST)) { remove(FTP_LISTING); } sprintf(msg,"OK=%d No_File=%d Skip=%d Error=%d (Time=%.1f s)",n[0],n[1],n[2], n[3],(tickget()-tick)*0.001); free_path(&paths); return 1; }
/* decode receiver raw/rtcm data ---------------------------------------------*/ static int decoderaw(rtksvr_t *svr, int index) { obs_t *obs; nav_t *nav; sbsmsg_t *sbsmsg=NULL; int i,ret,sat,fobs=0; tracet(4,"decoderaw: index=%d\n",index); rtksvrlock(svr); for (i=0;i<svr->nb[index];i++) { /* input rtcm/receiver raw data from stream */ if (svr->format[index]==STRFMT_RTCM2) { ret=input_rtcm2(svr->rtcm+index,svr->buff[index][i]); obs=&svr->rtcm[index].obs; nav=&svr->rtcm[index].nav; sat=svr->rtcm[index].ephsat; } else if (svr->format[index]==STRFMT_RTCM3) { ret=input_rtcm3(svr->rtcm+index,svr->buff[index][i]); obs=&svr->rtcm[index].obs; nav=&svr->rtcm[index].nav; sat=svr->rtcm[index].ephsat; } else { ret=input_raw(svr->raw+index,svr->format[index],svr->buff[index][i]); obs=&svr->raw[index].obs; nav=&svr->raw[index].nav; sat=svr->raw[index].ephsat; sbsmsg=&svr->raw[index].sbsmsg; } #if 0 /* record for receiving tick */ if (ret==1) { trace(0,"%d %10d T=%s NS=%2d\n",index,tickget(), time_str(obs->data[0].time,0),obs->n); } #endif /* update rtk server */ if (ret>0) updatesvr(svr,ret,obs,nav,sat,sbsmsg,index,fobs); /* observation data received */ if (ret==1) { if (fobs<MAXOBSBUF) fobs++; else svr->prcout++; } } svr->nb[index]=0; rtksvrunlock(svr); return fobs; }
/* get source table -------------------------------------------------------*/ static char *getsrctbl(const char *path) { static int lock=0; AnsiString s; stream_t str; char *p=buff,msg[MAXSTRMSG]; int ns,stat,len=strlen(ENDSRCTBL); unsigned int tick=tickget(); if (lock) return NULL; else lock=1; strinit(&str); if (!stropen(&str,STR_NTRIPCLI,STR_MODE_R,path)) { lock=0; MainForm->ShowMsg("stream open error"); return NULL; } MainForm->ShowMsg("connecting..."); while(p<buff+MAXSRCTBL-1) { ns=strread(&str,p,buff+MAXSRCTBL-p-1); *(p+ns)='\0'; if (p-len-3>buff&&strstr(p-len-3,ENDSRCTBL)) break; p+=ns; Sleep(NTRIP_CYCLE); stat=strstat(&str,msg); MainForm->ShowMsg(msg); if (stat<0) break; if ((int)(tickget()-tick)>NTRIP_TIMEOUT) { MainForm->ShowMsg("response timeout"); break; } } strclose(&str); lock=0; return buff; }
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; }
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); }
/* 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; }