/* 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); } }
/* open output/log stream ------------------------------------------------------ * open output/log stream * args : rtksvr_t *svr IO rtk server * int index I output/log stream index * (3:solution 1,4:solution 2,5:log rover, * 6:log base station,7:log correction) * int str I output/log stream types (STR_???) * char *path I output/log stream path * solopt_t *solopt I solution options * return : status (1:ok 0:error) *-----------------------------------------------------------------------------*/ extern int rtksvropenstr(rtksvr_t *svr, int index, int str, const char *path, const solopt_t *solopt) { tracet(3,"rtksvropenstr: index=%d str=%d path=%s\n",index,str,path); if (index<3||index>7||!svr->state) return 0; rtksvrlock(svr); if (svr->stream[index].state>0) { rtksvrunlock(svr); return 0; } if (!stropen(svr->stream+index,str,STR_MODE_W,path)) { tracet(2,"stream open error: index=%d\n",index); rtksvrunlock(svr); return 0; } if (index<=4) { svr->solopt[index-3]=*solopt; /* write solution header to solution stream */ writesolhead(svr->stream+index,svr->solopt+index-3); } rtksvrunlock(svr); return 1; }
static void RtkServer__readsatant(JNIEnv* env, jclass thiz, jstring file) { struct native_ctx_t *nctx; pcvs_t pcvs={0}; pcv_t *pcv; int i; gtime_t now=timeget(); nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field); if (nctx == NULL) { LOGV("nctx is null"); return; } const char *filename = (*env)->GetStringUTFChars(env, file, 0); rtksvrlock(&nctx->rtksvr); if (readpcv(filename,&pcvs)) { for (i=0;i<MAXSAT;i++) { if (!(pcv=searchpcv(i+1,"",now,&pcvs))) continue; nctx->rtksvr.nav.pcvs[i]=*pcv; } } rtksvrunlock(&nctx->rtksvr); (*env)->ReleaseStringUTFChars(env,file, filename); }
static void RtkServer__readsp3(JNIEnv* env, jclass thiz, jstring file) { struct native_ctx_t *nctx; nav_t nav={0}; rtksvr_t *svr; nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field); if (nctx == NULL) { LOGV("nctx is null"); return; } const char *filename = (*env)->GetStringUTFChars(env, file, 0); readsp3(filename,&nav,0); /* test if there is some ephemeris */ if (nav.ne<=0) { tracet(1,"sp3 file read error: %s\n",file); return; } //OK so update to server svr = &nctx->rtksvr; rtksvrlock(svr); if (svr->nav.peph) free(svr->nav.peph); svr->nav.ne=svr->nav.nemax=nav.ne; svr->nav.peph=nav.peph; rtksvrunlock(svr); (*env)->ReleaseStringUTFChars(env,file, filename); }
/* write solution to output stream -------------------------------------------*/ static void writesol(rtksvr_t *svr, int index) { solopt_t solopt=solopt_default; unsigned char buff[1024]; int i,n; tracet(4,"writesol: index=%d\n",index); for (i=0;i<2;i++) { /* output solution */ n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,svr->solopt+i); strwrite(svr->stream+i+3,buff,n); /* save output buffer */ saveoutbuf(svr,buff,n,i); /* output extended solution */ n=outsolexs(buff,&svr->rtk.sol,svr->rtk.ssat,svr->solopt+i); strwrite(svr->stream+i+3,buff,n); /* save output buffer */ saveoutbuf(svr,buff,n,i); } /* output solution to monitor port */ if (svr->moni) { n=outsols(buff,&svr->rtk.sol,svr->rtk.rb,&solopt); strwrite(svr->moni,buff,n); } /* save solution buffer */ if (svr->nsol<MAXSOLBUF) { rtksvrlock(svr); svr->solbuf[svr->nsol++]=svr->rtk.sol; rtksvrunlock(svr); } }
/* get observation data status ------------------------------------------------- * get current observation data status * args : rtksvr_t *svr I rtk server * int rcv I receiver (0:rover,1:base,2:ephem) * gtime_t *time O time of observation data * int *sat O satellite prn numbers * double *az O satellite azimuth angles (rad) * double *el O satellite elevation angles (rad) * int **snr O satellite snr for each freq (dBHz) * snr[i][j] = sat i freq j snr * int *vsat O valid satellite flag * return : number of satellites *-----------------------------------------------------------------------------*/ extern int rtksvrostat(rtksvr_t *svr, int rcv, gtime_t *time, int *sat, double *az, double *el, int **snr, int *vsat) { int i,j,ns; tracet(4,"rtksvrostat: rcv=%d\n",rcv); if (!svr->state) return 0; rtksvrlock(svr); ns=svr->obs[rcv][0].n; if (ns>0) { *time=svr->obs[rcv][0].data[0].time; } for (i=0;i<ns;i++) { sat [i]=svr->obs[rcv][0].data[i].sat; az [i]=svr->rtk.ssat[sat[i]-1].azel[0]; el [i]=svr->rtk.ssat[sat[i]-1].azel[1]; for (j=0;j<NFREQ;j++) { snr[i][j]=(int)(svr->obs[rcv][0].data[i].SNR[j]*0.25); } if (svr->rtk.sol.stat==SOLQ_NONE||svr->rtk.sol.stat==SOLQ_SINGLE) { vsat[i]=svr->rtk.ssat[sat[i]-1].vs; } else { vsat[i]=svr->rtk.ssat[sat[i]-1].vsat[0]; } } rtksvrunlock(svr); return ns; }
void baseStationCallback(const std_msgs::ByteMultiArray::ConstPtr& msg) { int n = msg->data.size(); unsigned char *p = server.buff[RTK_BASE_STATION]+server.nb[RTK_BASE_STATION]; unsigned char *q = server.buff[RTK_BASE_STATION]+server.buffsize; if(server.nb[RTK_BASE_STATION] + n < server.buffsize) { for(int i=0 ; i<n ; i++) { *(server.buff[RTK_BASE_STATION] + server.nb[RTK_BASE_STATION] + i) = msg->data[i]; } server.nb[RTK_BASE_STATION] += n; /* write receiver raw/rtcm data to log stream */ strwrite(server.stream+6, p, n); server.nb[RTK_BASE_STATION] += n; /* save peek buffer */ rtksvrlock(&server); n = n < server.buffsize - server.npb[RTK_BASE_STATION] ? n : server.buffsize - server.npb[RTK_BASE_STATION]; memcpy(server.pbuf[RTK_BASE_STATION] + server.npb[RTK_BASE_STATION], p, n); server.npb[RTK_BASE_STATION] += n; rtksvrunlock(&server); } }
/* save output buffer --------------------------------------------------------*/ static void saveoutbuf(rtksvr_t *svr, unsigned char *buff, int n, int index) { rtksvrlock(svr); n=n<svr->buffsize-svr->nsb[index]?n:svr->buffsize-svr->nsb[index]; memcpy(svr->sbuf[index]+svr->nsb[index],buff,n); svr->nsb[index]+=n; rtksvrunlock(svr); }
/* close output/log stream ----------------------------------------------------- * close output/log stream * args : rtksvr_t *svr IO rtk server * int index I output/log stream index * (3:solution 1,4:solution 2,5:log rover, * 6:log base station,7:log correction) * return : none *-----------------------------------------------------------------------------*/ extern void rtksvrclosestr(rtksvr_t *svr, int index) { tracet(3,"rtksvrclosestr: index=%d\n",index); if (index<3||index>7||!svr->state) return; rtksvrlock(svr); strclose(svr->stream+index); rtksvrunlock(svr); }
/* 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; }
static void RtkServer__get_rtk_status(JNIEnv* env, jclass thiz, jobject j_rtk_control_result) { struct native_ctx_t *nctx; nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field); if (nctx == NULL) { LOGV("nctx is null"); return; } rtksvrlock(&nctx->rtksvr); set_rtk_status(env, j_rtk_control_result, &nctx->rtksvr.rtk); rtksvrunlock(&nctx->rtksvr); }
static void RtkServer__read_solution_buffer(JNIEnv* env, jclass thiz, jobject j_solbuf) { struct native_ctx_t *nctx; nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field); if (nctx == NULL) { LOGV("nctx is null"); return; } rtksvrlock(&nctx->rtksvr); set_solution_buffer(env, j_solbuf, nctx->rtksvr.solbuf, nctx->rtksvr.nsol); nctx->rtksvr.nsol=0; rtksvrunlock(&nctx->rtksvr); }
/* get stream status ----------------------------------------------------------- * get current stream status * args : rtksvr_t *svr I rtk server * int *sstat O status of streams * char *msg O status messages * return : none *-----------------------------------------------------------------------------*/ extern void rtksvrsstat(rtksvr_t *svr, int *sstat, char *msg) { int i; char s[MAXSTRMSG],*p=msg; INIT_ZERO(s); tracet(4,"rtksvrsstat:\n"); rtksvrlock(svr); for (i=0;i<MAXSTRRTK;i++) { sstat[i]=strstat(svr->stream+i,s); if (*s) p+=sprintf(p,"(%d) %s ",i+1,s); } rtksvrunlock(svr); }
static void RtkServer__write_commands(JNIEnv* env, jclass thiz, jobjectArray j_cmds) { unsigned i; struct native_ctx_t *nctx; const char *cmds[3] = {NULL, NULL, NULL}; jstring cmds_jstring[3] = {NULL, NULL, NULL}; LOGV("RtkServer__write_start_commands() "); nctx = (struct native_ctx_t *)(uintptr_t)(*env)->GetLongField(env, thiz, m_object_field); if (nctx == NULL) { LOGV("nctx is null"); return; } for (i=0; i<sizeof(cmds)/sizeof(cmds[0]); ++i) { cmds_jstring[i] = (*env)->GetObjectArrayElement(env, j_cmds, i); if ((*env)->ExceptionOccurred(env)) goto write_commands_end; if (cmds_jstring[i] != NULL) { cmds[i] = (*env)->GetStringUTFChars(env, cmds_jstring[i], NULL); if (cmds[i] == NULL) goto write_commands_end; } } rtksvrlock(&nctx->rtksvr); if (cmds[0]) strsendcmd(&nctx->rtksvr.stream[0], cmds[0]); if (cmds[1]) strsendcmd(&nctx->rtksvr.stream[1], cmds[1]); if (cmds[2]) strsendcmd(&nctx->rtksvr.stream[2], cmds[2]); rtksvrunlock(&nctx->rtksvr); write_commands_end: for (i=0; i<sizeof(cmds)/sizeof(cmds[0]); ++i) { if (cmds[i] != NULL) (*env)->ReleaseStringUTFChars(env, cmds_jstring[i], cmds[i]); } }
} char get_solution(double &lat, double &lon, double &height) { rtksvrlock(&svr); const sol_t *sol = &svr.solbuf[svr.nsol-1]; const double *rb = svr.rtk.rb; double pos[3]={0},Qr[9],Qe[9]={0},dms1[3]={0},dms2[3]={0},bl[3]={0}; double enu[3]={0},pitch=0.0,yaw=0.0,len; int i; int status; if (sol->time.time == 0 || !sol->stat) { rtksvrunlock(&svr); return -1; // no fix } if (1 <= sol->stat && sol->stat <= 2) status = 0; // fix if (sol->stat == 3) status = 1; // sbas fix else status = 2; // differential fix if (norm(sol->rr,3) > 0.0 && norm(rb,3) > 0.0) { for (i=0;i<3;i++) bl[i]=sol->rr[i]-rb[i]; } len=norm(bl,3); Qr[0]=sol->qr[0]; Qr[4]=sol->qr[1]; Qr[8]=sol->qr[2]; Qr[1]=Qr[3]=sol->qr[3]; Qr[5]=Qr[7]=sol->qr[4]; Qr[2]=Qr[6]=sol->qr[5]; if (norm(sol->rr,3) > 0.0) { ecef2pos(sol->rr,pos); covenu(pos,Qr,Qe); lat = pos[0]*R2D; lon = pos[1]*R2D; if (solopt[0].height == 1) pos[2]-=geoidh(pos); /* geodetic */ height = pos[2]; } svr.nsol=0; rtksvrunlock(&svr);
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 }
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; }