示例#1
0
/* process positioning -------------------------------------------------------*/
static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt,
                    int mode)
{
    gtime_t time={0};
    sol_t sol={{0}};
    rtk_t rtk;
    obsd_t obs[MAXOBS];
    double rb[3]={0};
    int i,nobs,n,solstatic,pri[]={0,1,2,3,4,5,1,6};
    
    trace(3,"procpos : mode=%d\n",mode);
    
    solstatic=sopt->solstatic&&
              (popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC);
    
    rtkinit(&rtk,popt);
    rtcm_path[0]='\0';
    
    while ((nobs=inputobs(obs,rtk.sol.stat,popt))>=0) {
        
        /* exclude satellites */
        for (i=n=0;i<nobs;i++) {
            if ((satsys(obs[i].sat,NULL)&popt->navsys)&&
                popt->exsats[obs[i].sat-1]!=1) obs[n++]=obs[i];
        }
        if (n<=0) continue;
        
        if (!rtkpos(&rtk,obs,n,&navs)) continue;
        
        if (mode==0) { /* forward/backward */
            if (!solstatic) {
                outsol(fp,&rtk.sol,rtk.rb,sopt);
            }
            else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) {
                sol=rtk.sol;
                for (i=0;i<3;i++) rb[i]=rtk.rb[i];
                if (time.time==0||timediff(rtk.sol.time,time)<0.0) {
                    time=rtk.sol.time;
                }
            }
        }
        else if (!revs) { /* combined-forward */
            if (isolf>=nepoch) return;
            solf[isolf]=rtk.sol;
            for (i=0;i<3;i++) rbf[i+isolf*3]=rtk.rb[i];
            isolf++;
        }
        else { /* combined-backward */
            if (isolb>=nepoch) return;
            solb[isolb]=rtk.sol;
            for (i=0;i<3;i++) rbb[i+isolb*3]=rtk.rb[i];
            isolb++;
        }
    }
    if (mode==0&&solstatic&&time.time!=0.0) {
        sol.time=time;
        outsol(fp,&sol,rb,sopt);
    }
    rtkfree(&rtk);
}
示例#2
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
}
示例#3
0
/* input obs data, navigation messages and sbas correction -------------------*/
static int inputobs(obsd_t *obs, int solq, const prcopt_t *popt)
{
    gtime_t time={0};
    int i,nu,nr,n=0;
char str[32];
    
    trace(3,"infunc  : revs=%d iobsu=%d iobsr=%d isbs=%d\n",revs,iobsu,iobsr,isbs);
    
    if (0<=iobsu&&iobsu<obss.n) {
        settime((time=obss.data[iobsu].time));
        if (checkbrk("processing : %s Q=%d",time_str(time,0),solq)) {
            aborts=1; showmsg("aborted"); return -1;
        }
    }
    if (!revs) { /* input forward data */
        if ((nu=nextobsf(&obss,&iobsu,1))<=0) return -1;
        if (popt->intpref) {
            for (;(nr=nextobsf(&obss,&iobsr,2))>0;iobsr+=nr)
                if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)>-DTTOL) break;
        }
        else {
            for (i=iobsr;(nr=nextobsf(&obss,&i,2))>0;iobsr=i,i+=nr)
                if (timediff(obss.data[i].time,obss.data[iobsu].time)>DTTOL) break;
        }
        nr=nextobsf(&obss,&iobsr,2);
        for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu+i];
        for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr+i];
        iobsu+=nu;
        
        /* update sbas corrections */
        while (isbs<sbss.n) {
            time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow);
            
            if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */
                sbsupdatecorr(sbss.msgs+isbs,&navs);
            }
            if (timediff(time,obs[0].time)>-1.0-DTTOL) break;
            isbs++;
        }
        /* update lex corrections */
        while (ilex<lexs.n) {
            if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) {
                if (timediff(time,obs[0].time)>-1.0-DTTOL) break;
            }
            ilex++;
        }
        /* update rtcm corrections */
        if (fp_rtcm) {
            if (rtcm.time.time==0) rtcm.time=obs[0].time;
            
#if 1
            while (timediff(rtcm.time,obs[0].time)<-1.0) {
#else
            while (timediff(rtcm.time,obs[0].time)<5.0) {
#endif
                if (input_rtcm3f(&rtcm,fp_rtcm)==-2) break;
            }
            for (i=0;i<MAXSAT;i++) navs.ssr[i]=rtcm.ssr[i];
        }
    }
    else { /* input backward data */
        if ((nu=nextobsb(&obss,&iobsu,1))<=0) return -1;
        if (popt->intpref) {
            for (;(nr=nextobsb(&obss,&iobsr,2))>0;iobsr-=nr)
                if (timediff(obss.data[iobsr].time,obss.data[iobsu].time)<DTTOL) break;
        }
        else {
            for (i=iobsr;(nr=nextobsb(&obss,&i,2))>0;iobsr=i,i-=nr)
                if (timediff(obss.data[i].time,obss.data[iobsu].time)<-DTTOL) break;
        }
        nr=nextobsb(&obss,&iobsr,2);
        for (i=0;i<nu&&n<MAXOBS;i++) obs[n++]=obss.data[iobsu-nu+1+i];
        for (i=0;i<nr&&n<MAXOBS;i++) obs[n++]=obss.data[iobsr-nr+1+i];
        iobsu-=nu;
        
        /* update sbas corrections */
        while (isbs>=0) {
            time=gpst2time(sbss.msgs[isbs].week,sbss.msgs[isbs].tow);
            
            if (getbitu(sbss.msgs[isbs].msg,8,6)!=9) { /* except for geo nav */
                sbsupdatecorr(sbss.msgs+isbs,&navs);
            }
            if (timediff(time,obs[0].time)<1.0+DTTOL) break;
            isbs--;
        }
        /* update lex corrections */
        while (ilex>=0) {
            if (lexupdatecorr(lexs.msgs+ilex,&navs,&time)) {
                if (timediff(time,obs[0].time)<1.0+DTTOL) break;
            }
            ilex--;
        }
    }
    return n;
}
/* process positioning -------------------------------------------------------*/
static void procpos(FILE *fp, const prcopt_t *popt, const solopt_t *sopt,
                    int mode)
{
    gtime_t time={0};
    sol_t sol={{0}};
    rtk_t rtk;
    obsd_t obs[MAXOBS];
    double rb[3]={0};
    int i,nobs,n,solstatic,pri[]={0,1,2,3,4,5,1,6};
    
    trace(3,"procpos : mode=%d\n",mode);
    
    solstatic=sopt->solstatic&&
              (popt->mode==PMODE_STATIC||popt->mode==PMODE_PPP_STATIC);
    
    rtkinit(&rtk,popt);
    
    while ((nobs=inputobs(obs,rtk.sol.stat,popt))>=0) {
        
        /* exclude satellites */
        for (i=n=0;i<nobs;i++) {
            if ((satsys(obs[i].sat,NULL)&popt->navsys)&&
                popt->exsats[obs[i].sat-1]!=1) obs[n++]=obs[i];
        }
        if (n<=0) continue;
        
        if (!rtkpos(&rtk,obs,n,&navs)) continue;
        
        if (mode==0) { /* forward/backward */
            if (!solstatic) {
                outsol(fp,&rtk.sol,rtk.rb,sopt);
            }
            else if (time.time==0||pri[rtk.sol.stat]<=pri[sol.stat]) {
                sol=rtk.sol;
                for (i=0;i<3;i++) rb[i]=rtk.rb[i];
                if (time.time==0||timediff(rtk.sol.time,time)<0.0) {
                    time=rtk.sol.time;
                }
            }
        }
        else if (!revs) { /* combined-forward */
            if (isolf>=nepoch) return;
            solf[isolf]=rtk.sol;
            for (i=0;i<3;i++) rbf[i+isolf*3]=rtk.rb[i];
            isolf++;
        }
        else { /* combined-backward */
            if (isolb>=nepoch) return;
            solb[isolb]=rtk.sol;
            for (i=0;i<3;i++) rbb[i+isolb*3]=rtk.rb[i];
            isolb++;
        }
    }
    if (mode==0&&solstatic&&time.time!=0.0) {
        sol.time=time;
        outsol(fp,&sol,rb,sopt);
    }
    rtkfree(&rtk);
}
示例#4
0
/* initialize rtk server -------------------------------------------------------
* initialize rtk server
* args   : rtksvr_t *svr    IO rtk server
* return : status (0:error,1:ok)
*-----------------------------------------------------------------------------*/
extern int rtksvrinit(rtksvr_t *svr)
{
    gtime_t time0;
    sol_t  sol0;
    eph_t  eph0;
    geph_t geph0;
    seph_t seph0;
    int i,j;
    
    INIT_ZERO(time0);
    INIT_ZERO(sol0);
    INIT_ZERO(eph0);
    INIT_ZERO(geph0);
    INIT_ZERO(seph0);



    tracet(3,"rtksvrinit:\n");
    
    svr->state=svr->cycle=svr->nmeacycle=svr->nmeareq=0;
    for (i=0;i<3;i++) svr->nmeapos[i]=0.0;
    svr->buffsize=0;
    for (i=0;i<3;i++) svr->format[i]=0;
    for (i=0;i<2;i++) svr->solopt[i]=solopt_default;
    svr->navsel=svr->nsbs=svr->nsol=0;
    rtkinit(&svr->rtk,&prcopt_default);
    for (i=0;i<3;i++) svr->nb[i]=0;
    for (i=0;i<2;i++) svr->nsb[i]=0;
    for (i=0;i<3;i++) svr->npb[i]=0;
    for (i=0;i<3;i++) svr->buff[i]=NULL;
    for (i=0;i<2;i++) svr->sbuf[i]=NULL;
    for (i=0;i<3;i++) svr->pbuf[i]=NULL;
    for (i=0;i<MAXSOLBUF;i++) svr->solbuf[i]=sol0;
    for (i=0;i<3;i++) for (j=0;j<10;j++) svr->nmsg[i][j]=0;
    for (i=0;i<3;i++) svr->ftime[i]=time0;
    for (i=0;i<3;i++) svr->files[i][0]='\0';
    svr->moni=NULL;
    svr->tick=0;
    svr->thread=0;
    svr->cputime=svr->prcout=0;
    
    if (!(svr->nav.eph =(eph_t  *)malloc(sizeof(eph_t )*MAXSAT *2))||
        !(svr->nav.geph=(geph_t *)malloc(sizeof(geph_t)*NSATGLO*2))||
        !(svr->nav.seph=(seph_t *)malloc(sizeof(seph_t)*NSATSBS*2))) {
        tracet(1,"rtksvrinit: malloc error\n");
        return 0;
    }
    for (i=0;i<MAXSAT *2;i++) svr->nav.eph [i]=eph0;
    for (i=0;i<NSATGLO*2;i++) svr->nav.geph[i]=geph0;
    for (i=0;i<NSATSBS*2;i++) svr->nav.seph[i]=seph0;
    svr->nav.n =MAXSAT *2;
    svr->nav.ng=NSATGLO*2;
    svr->nav.ns=NSATSBS*2;
    
    for (i=0;i<3;i++) for (j=0;j<MAXOBSBUF;j++) {
        if (!(svr->obs[i][j].data=(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))) {
            tracet(1,"rtksvrinit: malloc error\n");
            return 0;
        }
    }
    for (i=0;i<3;i++) {
        memset(svr->raw +i,0,sizeof(raw_t ));
        memset(svr->rtcm+i,0,sizeof(rtcm_t));
    }
    for (i=0;i<MAXSTRRTK;i++) strinit(svr->stream+i);
    
    initlock(&svr->lock);
    
    return 1;
}
示例#5
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);
}