Ejemplo n.º 1
0
static void statesend(hcnraw_t *hcnraw){

    state_t state = { { 0 } };
    gtime_t utc;
    double timer[6];

      utc=gpst2utc(hcnraw->time);
      time2epoch(utc,timer);
#if 0
      if((timer=gmtime(&utc.time))){
          sprintf(state.utctime,"%d-%d-%d-%d-%d-%d",timer->tm_year,
                  timer->tm_mon,timer->tm_mday,timer->tm_hour,timer->tm_min,
                  timer->tm_sec);
      }
#endif
      sprintf(state.utctime,"%.0f-%.0f-%.0f-%.0f-%.0f-%.0f",
              timer[0],timer[1],timer[2],timer[3],timer[4],timer[5]);
      state.antn.mainantnworksta = hcnraw->board.mainantnworksta;
      state.antn.mainantnrfsta   = hcnraw->board.mainantnrfsta;
      state.antn.aantnworksta    = hcnraw->board.aantnworksta;
      state.antn.aantnrfsta      = hcnraw->board.aantnrfsta;
      state.cpu.acpuload         = hcnraw->board.acpuload;
      state.cpu.dcpuload         = hcnraw->board.dcpuload;
      state.cpu.astatus          = hcnraw->board.astatus;
      state.cpu.dstatus          = hcnraw->board.dstatus;
#if 1
      state.cpu.fstatus          = hcnraw->board.fstatus;
#endif
      pthread_mutex_lock(&lock);
      cmd_response_massage( sock,&msg_head[0],&state,sizeof(state_t));
      pthread_mutex_unlock(&lock);

}
Ejemplo n.º 2
0
/* exclude meas of eclipsing satellite (block IIA) ---------------------------*/
static void testeclipse(const obsd_t *obs, int n, const nav_t *nav, double *rs)
{
    double rsun[3],esun[3],r,ang,erpv[5]={0},cosa;
    int i,j;
    const char *type;
    
    trace(3,"testeclipse:\n");
    
    /* unit vector of sun direction (ecef) */
    sunmoonpos(gpst2utc(obs[0].time),erpv,rsun,NULL,NULL);
    normv3(rsun,esun);
    
    for (i=0;i<n;i++) {
        type=nav->pcvs[obs[i].sat-1].type;
        
        if ((r=norm(rs+i*6,3))<=0.0) continue;
#if 1
        /* only block IIA */
        if (*type&&!strstr(type,"BLOCK IIA")) continue;
#endif
        /* sun-earth-satellite angle */
        cosa=dot(rs+i*6,esun,3)/r;
        cosa=cosa<-1.0?-1.0:(cosa>1.0?1.0:cosa);
        ang=acos(cosa);
        
        /* test eclipse */
        if (ang<PI/2.0||r*sin(ang)>RE_WGS84) continue;
        
        trace(2,"eclipsing sat excluded %s sat=%2d\n",time_str(obs[0].time,0),
              obs[i].sat);
        
        for (j=0;j<3;j++) rs[j+i*6]=0.0;
    }
}
Ejemplo n.º 3
0
// get position regarding time ----------------------------------------------
double __fastcall TPlot::TimePos(gtime_t time)
{
    double tow;
    int week;
    
    if (TimeLabel<=1) { // www/ssss or gpst
        tow=time2gpst(time,&week);
    }
    else if (TimeLabel==2) { // utc
        tow=time2gpst(gpst2utc(time),&week);
    }
    else { // jst
        tow=time2gpst(timeadd(gpst2utc(time),9*3600.0),&week);
    }
    return tow+(week-Week)*86400.0*7;
}
Ejemplo n.º 4
0
/* gpst2utc() */
void utest7(void)
{
    double ep0[]={1980, 1, 6, 0, 0, 0.000000};
    double ep1[]={1992, 7, 1, 0, 0, 6.999999};
    double ep2[]={1992, 7, 1, 0, 0, 7.000000};
    double ep3[]={1992, 7, 1, 0, 0, 8.000000};
    double ep4[]={2004,12,31,23,59,59.999999};
    double ep5[]={2006, 1, 1, 0, 0, 0.000000};
    double ep6[]={2038, 1, 1, 0, 0, 0.000000};
    gtime_t t;
    double ep[6];
    t=gpst2utc(epoch2time(ep0)); time2epoch(t,ep);
        assert(ep[0]==1980&&ep[1]==1&&ep[2]==6&&ep[3]==0&&ep[4]==0&&ep[5]==0.0);
    t=gpst2utc(epoch2time(ep1)); time2epoch(t,ep);
        assert(ep[0]==1992&&ep[1]==6&&ep[2]==30&&ep[3]==23&&ep[4]==59&&fabs(ep[5]-59.999999)<1E-14);
    t=gpst2utc(epoch2time(ep2)); time2epoch(t,ep);
        assert(ep[0]==1992&&ep[1]==7&&ep[2]==1&&ep[3]==0&&ep[4]==0&&ep[5]==0.0);
    t=gpst2utc(epoch2time(ep3)); time2epoch(t,ep);
        assert(ep[0]==1992&&ep[1]==7&&ep[2]==1&&ep[3]==0&&ep[4]==0&&ep[5]==0.0);
    t=gpst2utc(epoch2time(ep4)); time2epoch(t,ep);
        assert(ep[0]==2004&&ep[1]==12&&ep[2]==31&&ep[3]==23&&ep[4]==59&&fabs(ep[5]-46.999999)<1E-14);
    t=gpst2utc(epoch2time(ep5)); time2epoch(t,ep);
        assert(ep[0]==2005&&ep[1]==12&&ep[2]==31&&ep[3]==23&&ep[4]==59&&ep[5]==47.0);
    t=gpst2utc(epoch2time(ep6)); time2epoch(t,ep);
        assert(ep[0]==2037&&ep[1]==12&&ep[2]==31&&ep[3]==23&&ep[4]==59&&ep[5]==44.0);

    printf("%s utset7 : OK\n",__FILE__);
}
Ejemplo n.º 5
0
/* 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__);
}
Ejemplo n.º 6
0
// 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);
}
Ejemplo n.º 7
0
/* internal decode stream function */
static void decode_stream(raw_t *raw, unsigned char buff[], int len)
{
    /* local variable */
    int i, j, status;
    int sys, prn;
    gtime_t utctimebj;
    for (i=0; i<len; i++) {
        status = decode_unicore(raw, buff[i]);
        if (status <= 0) continue;
        /* get beijing utc time */
        utctimebj = gpst2utc(raw->time);
        utctimebj.time += 8*3600;
        /* get gsof_sat data */
        if (status == 24 ) {
            printf("%20s ==== azi & ele ====> %02d\n", time_str(utctimebj, 3),
                raw->gsof.sat.num);
            for(j=0; j<raw->gsof.sat.num; j++) {
                printf("    %c%02d  %8.3f %8.3f\n", 
                    (raw->gsof.sat.data[j].sys == SYS_GPS)? 'G': 'C',
                    raw->gsof.sat.data[j].prn,
                    raw->gsof.sat.data[j].azi,
                    raw->gsof.sat.data[j].ele);
            }
            printf("-----------------------------------------------------\n");
        }
        /* get raw_sna data */
        if (status == 1 ) {
            printf("%20s ==== sna ====> %02d\n", time_str(utctimebj, 3),
                raw->obs.n);
            for(j=0; j<raw->obs.n; j++) {
                sys = satsys(raw->obs.data[j].sat, &prn);
                printf("    %c%02d  %8.3f %8.3f\n", 
                    (sys == SYS_GPS)? 'G': 'C',
                    prn,
                    raw->obs.data[j].SNR[0]/4.0,
                    raw->obs.data[j].SNR[1]/4.0);
            }
            printf("-----------------------------------------------------\n");
        }
    }
}
Ejemplo n.º 8
0
//---------------------------------------------------------------------------
void __fastcall TTimeDialog::FormShow(TObject *Sender)
{
	gtime_t utc;
	double tow,doy;
	int week;
	char msg[1024],s1[64],s2[64],*p=msg;
	utc=gpst2utc(Time);
	time2str(Time,s1,0);
	time2str(utc,s2,0);
	tow=time2gpst(Time,&week);
	doy=time2doy(Time);
	p+=sprintf(p,"%s GPST\n",s1);
	p+=sprintf(p,"%s UTC\n\n",s2);
	p+=sprintf(p,"GPS Week: %d\n",week);
	p+=sprintf(p,"GPS Time: %.0f s\n",tow);
	p+=sprintf(p,"Day of Year: %03d\n",(int)floor(doy));
	p+=sprintf(p,"Day of Week: %d\n",(int)floor(tow/86400.0));
	p+=sprintf(p,"Time of Day: %.0f s\n",fmod(tow,86400.0));
	sprintf(p,"Leap Seconds: %.0f s\n",timediff(Time,utc));
	Message->Caption=msg;
}
Ejemplo n.º 9
0
/* satellite antenna phase center offset ---------------------------------------
* compute satellite antenna phase center offset in ecef
* args   : gtime_t time       I   time (gpst)
*          double *rs         I   satellite position and velocity (ecef)
*                                 {x,y,z,vx,vy,vz} (m|m/s)
*          pcv_t  *pcv        I   satellite antenna parameter
*          double *dant       I   satellite antenna phase center offset (ecef)
*                                 {dx,dy,dz} (m)
* return : none
*-----------------------------------------------------------------------------*/
extern void satantoff(gtime_t time, const double *rs, const pcv_t *pcv,
                      double *dant)
{
    double ex[3],ey[3],ez[3],es[3],r[3],rsun[3],gmst;
    int i;
    
    trace(4,"satantoff: time=%s\n",time_str(time,3));
    
    /* sun position in ecef */
    sunmoonpos(gpst2utc(time),NULL,rsun,NULL,&gmst);
    
    /* unit vectors of satellite fixed coordinates */
    for (i=0;i<3;i++) r[i]=-rs[i];
    if (!normv3(r,ez)) return;
    for (i=0;i<3;i++) r[i]=rsun[i]-rs[i];
    if (!normv3(r,es)) return;
    cross3(ez,es,r);
    if (!normv3(r,ey)) return;
    cross3(ey,ez,ex);
    
    for (i=0;i<3;i++) { /* use L1 value */
        dant[i]=pcv->off[0][0]*ex[i]+pcv->off[0][1]*ey[i]+pcv->off[0][2]*ez[i];
    }
}
Ejemplo n.º 10
0
/* phase and code residuals --------------------------------------------------*/
static int res_ppp(int iter, const obsd_t *obs, int n, const double *rs,
                   const double *dts, const double *vare, const int *svh,
                   const nav_t *nav, const double *x, rtk_t *rtk, double *v,
                   double *H, double *R, double *azel)
{
    prcopt_t *opt=&rtk->opt;
    double r,rr[3],disp[3],pos[3],e[3],meas[2],dtdx[3],dantr[NFREQ]={0};
    double dants[NFREQ]={0},var[MAXOBS*2],dtrp=0.0,vart=0.0,varm[2]={0};
    int i,j,k,sat,sys,nv=0,nx=rtk->nx,brk,tideopt;
    
    trace(3,"res_ppp : n=%d nx=%d\n",n,nx);
    
    for (i=0;i<MAXSAT;i++) rtk->ssat[i].vsat[0]=0;
    
    for (i=0;i<3;i++) rr[i]=x[i];
    
    /* earth tides correction */
    if (opt->tidecorr) {
        tideopt=opt->tidecorr==1?1:7; /* 1:solid, 2:solid+otl+pole */
        
        tidedisp(gpst2utc(obs[0].time),rr,tideopt,&nav->erp,opt->odisp[0],
                 disp);
        for (i=0;i<3;i++) rr[i]+=disp[i];
    }
    ecef2pos(rr,pos);
    
    for (i=0;i<n&&i<MAXOBS;i++) {
        sat=obs[i].sat;
        if (!(sys=satsys(sat,NULL))||!rtk->ssat[sat-1].vs) continue;
        
        /* geometric distance/azimuth/elevation angle */
        if ((r=geodist(rs+i*6,rr,e))<=0.0||
            satazel(pos,e,azel+i*2)<opt->elmin) continue;
        
        /* excluded satellite? */
        if (satexclude(obs[i].sat,svh[i],opt)) continue;
        
        /* tropospheric delay correction */
        if (opt->tropopt==TROPOPT_SAAS) {
            dtrp=tropmodel(obs[i].time,pos,azel+i*2,REL_HUMI);
            vart=SQR(ERR_SAAS);
        }
        else if (opt->tropopt==TROPOPT_SBAS) {
            dtrp=sbstropcorr(obs[i].time,pos,azel+i*2,&vart);
        }
        else if (opt->tropopt==TROPOPT_EST||opt->tropopt==TROPOPT_ESTG) {
            dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x+IT(opt),dtdx,&vart);
        }
        else if (opt->tropopt==TROPOPT_COR||opt->tropopt==TROPOPT_CORG) {
            dtrp=prectrop(obs[i].time,pos,azel+i*2,opt,x,dtdx,&vart);
        }
        /* satellite antenna model */
        if (opt->posopt[0]) {
            satantpcv(rs+i*6,rr,nav->pcvs+sat-1,dants);
        }
        /* receiver antenna model */
        antmodel(opt->pcvr,opt->antdel[0],azel+i*2,opt->posopt[1],dantr);
        
        /* phase windup correction */
        if (opt->posopt[2]) {
            windupcorr(rtk->sol.time,rs+i*6,rr,&rtk->ssat[sat-1].phw);
        }
        /* ionosphere and antenna phase corrected measurements */
        if (!corrmeas(obs+i,nav,pos,azel+i*2,&rtk->opt,dantr,dants,
                      rtk->ssat[sat-1].phw,meas,varm,&brk)) {
            continue;
        }
        /* satellite clock and tropospheric delay */
        r+=-CLIGHT*dts[i*2]+dtrp;
        
        trace(5,"sat=%2d azel=%6.1f %5.1f dtrp=%.3f dantr=%6.3f %6.3f dants=%6.3f %6.3f phw=%6.3f\n",
              sat,azel[i*2]*R2D,azel[1+i*2]*R2D,dtrp,dantr[0],dantr[1],dants[0],
              dants[1],rtk->ssat[sat-1].phw);
        
        for (j=0;j<2;j++) { /* for phase and code */
            
            if (meas[j]==0.0) continue;
            
            for (k=0;k<nx;k++) H[k+nx*nv]=0.0;
            
            v[nv]=meas[j]-r;
            
            for (k=0;k<3;k++) H[k+nx*nv]=-e[k];
            
            if (sys!=SYS_GLO) {
                v[nv]-=x[IC(0,opt)];
                H[IC(0,opt)+nx*nv]=1.0;
            }
            else {
                v[nv]-=x[IC(1,opt)];
                H[IC(1,opt)+nx*nv]=1.0;
            }
            if (opt->tropopt>=TROPOPT_EST) {
                for (k=0;k<(opt->tropopt>=TROPOPT_ESTG?3:1);k++) {
                    H[IT(opt)+k+nx*nv]=dtdx[k];
                }
            }
            if (j==0) {
                v[nv]-=x[IB(obs[i].sat,opt)];
                H[IB(obs[i].sat,opt)+nx*nv]=1.0;
            }
            var[nv]=varerr(obs[i].sat,sys,azel[1+i*2],j,opt)+varm[j]+vare[i]+vart;
            
            if (j==0) rtk->ssat[sat-1].resc[0]=v[nv];
            else      rtk->ssat[sat-1].resp[0]=v[nv];
            
            /* test innovation */
#if 0
            if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno) {
#else
            if (opt->maxinno>0.0&&fabs(v[nv])>opt->maxinno&&sys!=SYS_GLO) {
#endif
                trace(2,"ppp outlier rejected %s sat=%2d type=%d v=%.3f\n",
                      time_str(obs[i].time,0),sat,j,v[nv]);
                rtk->ssat[sat-1].rejc[0]++;
                continue;
            }
            if (j==0) rtk->ssat[sat-1].vsat[0]=1;
            nv++;
        }
    }
    for (i=0;i<nv;i++) for (j=0;j<nv;j++) {
        R[i+j*nv]=i==j?var[i]:0.0;
    }
    trace(5,"x=\n"); tracemat(5,x, 1,nx,8,3);
    trace(5,"v=\n"); tracemat(5,v, 1,nv,8,3);
    trace(5,"H=\n"); tracemat(5,H,nx,nv,8,3);
    trace(5,"R=\n"); tracemat(5,R,nv,nv,8,5);
    return nv;
}
/* number of estimated states ------------------------------------------------*/
extern int pppnx(const prcopt_t *opt)
{
    return NX(opt);
}
/* precise point positioning -------------------------------------------------*/
extern void pppos(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav)
{
    const prcopt_t *opt=&rtk->opt;
    double *rs,*dts,*var,*v,*H,*R,*azel,*xp,*Pp;
    int i,nv,info,svh[MAXOBS],stat=SOLQ_SINGLE;
    
    trace(3,"pppos   : nx=%d n=%d\n",rtk->nx,n);
    
    rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel=zeros(2,n);
    
    for (i=0;i<MAXSAT;i++) rtk->ssat[i].fix[0]=0;
    
    /* temporal update of states */
    udstate_ppp(rtk,obs,n,nav);
    
    trace(4,"x(0)="); tracemat(4,rtk->x,1,NR(opt),13,4);
    
    /* satellite positions and clocks */
    satposs(obs[0].time,obs,n,nav,rtk->opt.sateph,rs,dts,var,svh);
    
    /* exclude measurements of eclipsing satellite */
    if (rtk->opt.posopt[3]) {
        testeclipse(obs,n,nav,rs);
    }
    xp=mat(rtk->nx,1); Pp=zeros(rtk->nx,rtk->nx);
    matcpy(xp,rtk->x,rtk->nx,1);
    nv=n*rtk->opt.nf*2; v=mat(nv,1); H=mat(rtk->nx,nv); R=mat(nv,nv);
    
    for (i=0;i<rtk->opt.niter;i++) {
        
        /* phase and code residuals */
        if ((nv=res_ppp(i,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel))<=0) break;
        
        /* measurement update */
        matcpy(Pp,rtk->P,rtk->nx,rtk->nx);
        
        if ((info=filter(xp,Pp,H,v,R,rtk->nx,nv))) {
            trace(2,"ppp filter error %s info=%d\n",time_str(rtk->sol.time,0),
                  info);
            break;
        }
        trace(4,"x(%d)=",i+1); tracemat(4,xp,1,NR(opt),13,4);
        
        stat=SOLQ_PPP;
    }
    if (stat==SOLQ_PPP) {
        /* postfit residuals */
        res_ppp(1,obs,n,rs,dts,var,svh,nav,xp,rtk,v,H,R,azel);
        
        /* update state and covariance matrix */
        matcpy(rtk->x,xp,rtk->nx,1);
        matcpy(rtk->P,Pp,rtk->nx,rtk->nx);
        
        /* ambiguity resolution in ppp */
        if (opt->modear==ARMODE_PPPAR||opt->modear==ARMODE_PPPAR_ILS) {
            if (pppamb(rtk,obs,n,nav,azel)) stat=SOLQ_FIX;
        }
        /* update solution status */
        rtk->sol.ns=0;
        for (i=0;i<n&&i<MAXOBS;i++) {
            if (!rtk->ssat[obs[i].sat-1].vsat[0]) continue;
            rtk->ssat[obs[i].sat-1].lock[0]++;
            rtk->ssat[obs[i].sat-1].outc[0]=0;
            rtk->ssat[obs[i].sat-1].fix [0]=4;
            rtk->sol.ns++;
        }
        rtk->sol.stat=stat;
        
        for (i=0;i<3;i++) {
            rtk->sol.rr[i]=rtk->x[i];
            rtk->sol.qr[i]=(float)rtk->P[i+i*rtk->nx];
        }
        rtk->sol.qr[3]=(float)rtk->P[1];
        rtk->sol.qr[4]=(float)rtk->P[2+rtk->nx];
        rtk->sol.qr[5]=(float)rtk->P[2];
        rtk->sol.dtr[0]=rtk->x[IC(0,opt)];
        rtk->sol.dtr[1]=rtk->x[IC(1,opt)]-rtk->x[IC(0,opt)];
        for (i=0;i<n&&i<MAXOBS;i++) {
            rtk->ssat[obs[i].sat-1].snr[0]=MIN(obs[i].SNR[0],obs[i].SNR[1]);
        }
        for (i=0;i<MAXSAT;i++) {
            if (rtk->ssat[i].slip[0]&3) rtk->ssat[i].slipc[0]++;
        }
    }
    free(rs); free(dts); free(var); free(azel);
    free(xp); free(Pp); free(v); free(H); free(R);
}
Ejemplo n.º 11
0
static jlong GTime_get_utc_time_millis(JNIEnv* env, jobject thiz)
{
   gtime_t t;
   t = gpst2utc(get_gtime_t(env, thiz));
   return (jlong)(1000ll * t.time + llround(1000.0 * t.sec));
}
Ejemplo n.º 12
0
static void *rtksvrthread(void *arg)
#endif
{
    rtksvr_t *svr=(rtksvr_t *)arg;
    obs_t obs;
    obsd_t data[MAXOBS*2];
    double tt;
    unsigned int tick,ticknmea;
    unsigned char *p,*q;
    int i,j,n,fobs[3],cycle,cputime;
    INIT_ZERO(fobs);

    
    tracet(3,"rtksvrthread:\n");
    
    svr->state=1; obs.data=data;
    svr->tick=tickget();
    ticknmea=svr->tick-1000;
    
    for (cycle=0;svr->state;cycle++) {
        tick=tickget();
        
        for (i=0;i<3;i++) {
            p=svr->buff[i]+svr->nb[i]; q=svr->buff[i]+svr->buffsize;
            
            /* read receiver raw/rtcm data from input stream */
            if ((n=strread(svr->stream+i,p,q-p))<=0) {
                continue;
            }
            /* write receiver raw/rtcm data to log stream */
            strwrite(svr->stream+i+5,p,n);
            svr->nb[i]+=n;
            
            /* save peek buffer */
            rtksvrlock(svr);
            n=n<svr->buffsize-svr->npb[i]?n:svr->buffsize-svr->npb[i];
            memcpy(svr->pbuf[i]+svr->npb[i],p,n);
            svr->npb[i]+=n;
            rtksvrunlock(svr);
        }
        for (i=0;i<3;i++) {
            if (svr->format[i]==STRFMT_SP3||svr->format[i]==STRFMT_RNXCLK) {
                /* decode download file */
                decodefile(svr,i);
            }
            else {
                /* decode receiver raw/rtcm data */
                fobs[i]=decoderaw(svr,i);
            }
        }
        for (i=0;i<fobs[0];i++) { /* for each rover observation data */
            obs.n=0;
            for (j=0;j<svr->obs[0][i].n&&obs.n<MAXOBS*2;j++) {
                obs.data[obs.n++]=svr->obs[0][i].data[j];
            }
            for (j=0;j<svr->obs[1][0].n&&obs.n<MAXOBS*2;j++) {
                obs.data[obs.n++]=svr->obs[1][0].data[j];
            }
            /* rtk positioning */
            rtksvrlock(svr);
            rtkpos(&svr->rtk,obs.data,obs.n,&svr->nav);
            rtksvrunlock(svr);
            
            if (svr->rtk.sol.stat!=SOLQ_NONE) {
                
                /* adjust current time */
                tt=(int)(tickget()-tick)/1000.0+DTTOL;
                timeset(gpst2utc(timeadd(svr->rtk.sol.time,tt)));
                
                /* write solution */
                writesol(svr,i);
            }
            /* if cpu overload, inclement obs outage counter and break */
            if ((int)(tickget()-tick)>=svr->cycle) {
                svr->prcout+=fobs[0]-i-1;
#if 0 /* omitted v.2.4.1 */
                break;
#endif
            }
        }
        /* send null solution if no solution (1hz) */
        if (svr->rtk.sol.stat==SOLQ_NONE&&cycle%(1000/svr->cycle)==0) {
            writesol(svr,0);
        }
        /* send nmea request to base/nrtk input stream */
        if (svr->nmeacycle>0&&(int)(tick-ticknmea)>=svr->nmeacycle) {
            if (svr->stream[1].state==1) {
                if (svr->nmeareq==1) {
                    strsendnmea(svr->stream+1,svr->nmeapos);
                }
                else if (svr->nmeareq==2&&norm(svr->rtk.sol.rr,3)>0.0) {
                    strsendnmea(svr->stream+1,svr->rtk.sol.rr);
                }
            }
            ticknmea=tick;
        }
        if ((cputime=(int)(tickget()-tick))>0) svr->cputime=cputime;
        
        /* sleep until next cycle */
        sleepms(svr->cycle-cputime);
    }
    for (i=0;i<MAXSTRRTK;i++) strclose(svr->stream+i);
    for (i=0;i<3;i++) {
        svr->nb[i]=svr->npb[i]=0;
        free(svr->buff[i]); svr->buff[i]=NULL;
        free(svr->pbuf[i]); svr->pbuf[i]=NULL;
        free_raw (svr->raw +i);
        free_rtcm(svr->rtcm+i);
    }
    for (i=0;i<2;i++) {
        svr->nsb[i]=0;
        free(svr->sbuf[i]); svr->sbuf[i]=NULL;
    }
    return 0;
}
Ejemplo n.º 13
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);
}
Ejemplo n.º 14
0
/* 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;
}