Beispiel #1
0
/* dual-frequency iono-free measurements -------------------------------------*/
static int ifmeas(const obsd_t *obs, const nav_t *nav, const double *azel,
                  const prcopt_t *opt, const double *dantr, const double *dants,
                  double phw, double *meas, double *var)
{
    const double *lam=nav->lam[obs->sat-1];
    double c1,c2,L1,L2,P1,P2,P1_C1,P2_C2,gamma;
    int i=0,j=1,k;
    
    trace(4,"ifmeas  :\n");
    
    /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */
    if (NFREQ>=3&&(satsys(obs->sat,NULL)&(SYS_GAL|SYS_SBS))) j=2;
    
    if (NFREQ<2||lam[i]==0.0||lam[j]==0.0) return 0;
    
    /* test snr mask */
    if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)||
        testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) {
        return 0;
    }
    gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */
    c1=gamma/(gamma-1.0);  /*  f1^2/(f1^2-f2^2) */
    c2=-1.0 /(gamma-1.0);  /* -f2^2/(f1^2-f2^2) */
    
    L1=obs->L[i]*lam[i]; /* cycle -> m */
    L2=obs->L[j]*lam[j];
    P1=obs->P[i];
    P2=obs->P[j];
    P1_C1=nav->cbias[obs->sat-1][1];
    P2_C2=nav->cbias[obs->sat-1][2];
    if (opt->sateph==EPHOPT_LEX) {
        P1_C1=nav->lexeph[obs->sat-1].isc[0]*CLIGHT; /* ISC_L1C/A */
    }
    if (L1==0.0||L2==0.0||P1==0.0||P2==0.0) return 0;
    
    /* iono-free phase with windup correction */
    meas[0]=c1*L1+c2*L2-(c1*lam[i]+c2*lam[j])*phw;
    
    /* iono-free code with dcb correction */
    if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
    if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */
    meas[1]=c1*P1+c2*P2;
    var[1]=SQR(ERR_CBIAS);
    
    if (opt->sateph==EPHOPT_SBAS) meas[1]-=P1_C1; /* sbas clock based C1 */
    
    /* gps-glonass h/w bias correction for code */
    if (opt->exterr.ena[3]&&satsys(obs->sat,NULL)==SYS_GLO) {
        meas[1]+=c1*opt->exterr.gpsglob[0]+c2*opt->exterr.gpsglob[1];
    }
    /* antenna phase center variation correction */
    for (k=0;k<2;k++) {
        if (dants) meas[k]-=c1*dants[i]+c2*dants[j];
        if (dantr) meas[k]-=c1*dantr[i]+c2*dantr[j];
    }
    return 1;
}
Beispiel #2
0
/* satellite clock with broadcast ephemeris ----------------------------------*/
static int ephclk(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
                  double *dts)
{
    eph_t  *eph;
    geph_t *geph;
    seph_t *seph;
    int sys;
    
    trace(4,"ephclk  : time=%s sat=%2d\n",time_str(time,3),sat);
    
    sys=satsys(sat,NULL);
    
    if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
        if (!(eph=seleph(teph,sat,-1,nav))) return 0;
        *dts=eph2clk(time,eph);
    }
    else if (sys==SYS_GLO) {
        if (!(geph=selgeph(teph,sat,-1,nav))) return 0;
        *dts=geph2clk(time,geph);
    }
    else if (sys==SYS_SBS) {
        if (!(seph=selseph(teph,sat,nav))) return 0;
        *dts=seph2clk(time,seph);
    }
    else return 0;
    
    return 1;
}
Beispiel #3
0
/* select ephememeris --------------------------------------------------------*/
static eph_t *seleph(gtime_t time, int sat, int iode, const nav_t *nav)
{
    double t,tmax,tmin;
    int i,j=-1;
    
    trace(4,"seleph  : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
    
    switch (satsys(sat,NULL)) {
        case SYS_QZS: tmax=MAXDTOE_QZS+1.0; break;
        case SYS_GAL: tmax=MAXDTOE_GAL+1.0; break;
        case SYS_CMP: tmax=MAXDTOE_CMP+1.0; break;
        default: tmax=MAXDTOE+1.0; break;
    }
    tmin=tmax+1.0;
    
    for (i=0;i<nav->n;i++) {
        if (nav->eph[i].sat!=sat) continue;
        if (iode>=0&&nav->eph[i].iode!=iode) continue;
        if ((t=fabs(timediff(nav->eph[i].toe,time)))>tmax) continue;
        if (iode>=0) return nav->eph+i;
        if (t<=tmin) {j=i; tmin=t;} /* toe closest to time */
    }
    if (iode>=0||j<0) {
        trace(2,"no broadcast ephemeris: %s sat=%2d iode=%3d\n",time_str(time,0),
              sat,iode);
        return NULL;
    }
    return nav->eph+j;
}
Beispiel #4
0
/* almanac to satellite position and clock bias --------------------------------
* compute satellite position and clock bias with almanac (gps, galileo, qzss)
* args   : gtime_t time     I   time (gpst)
*          alm_t *alm       I   almanac
*          double *rs       O   satellite position (ecef) {x,y,z} (m)
*          double *dts      O   satellite clock bias (s)
* return : none
* notes  : see ref [1],[7],[8]
*-----------------------------------------------------------------------------*/
extern void alm2pos(gtime_t time, const alm_t *alm, double *rs, double *dts)
{
    double tk,M,E,Ek,sinE,cosE,u,r,i,O,x,y,sinO,cosO,cosi,mu;
    
    trace(4,"alm2pos : time=%s sat=%2d\n",time_str(time,3),alm->sat);
    
    tk=timediff(time,alm->toa);
    
    if (alm->A<=0.0) {
        rs[0]=rs[1]=rs[2]=*dts=0.0;
        return;
    }
    mu=satsys(alm->sat,NULL)==SYS_GAL?MU_GAL:MU_GPS;
    
    M=alm->M0+sqrt(mu/(alm->A*alm->A*alm->A))*tk;
    for (E=M,sinE=Ek=0.0;fabs(E-Ek)>1E-12;) {
        Ek=E; sinE=sin(Ek); E=M+alm->e*sinE;
    }
    cosE=cos(E);
    u=atan2(sqrt(1.0-alm->e*alm->e)*sinE,cosE-alm->e)+alm->omg;
    r=alm->A*(1.0-alm->e*cosE);
    i=alm->i0;
    O=alm->OMG0+(alm->OMGd-OMGE)*tk-OMGE*alm->toas;
    x=r*cos(u); y=r*sin(u); sinO=sin(O); cosO=cos(O); cosi=cos(i);
    rs[0]=x*cosO-y*cosi*sinO;
    rs[1]=x*sinO+y*cosi*cosO;
    rs[2]=y*sin(i);
    *dts=alm->f0+alm->f1*tk;
}
Beispiel #5
0
/* decode skytraq subframe buffer --------------------------------------------*/
static int decode_stqsfrb(raw_t *raw)
{
    int prn,sat,sys,id;
    unsigned char *p=raw->buff+4;
    
    trace(4,"decode_stqsfrb: len=%d\n",raw->len);
    
    if (raw->len<40) {
        trace(2,"stq subframe length error: len=%d\n",raw->len);
        return -1;
    }
    prn=U1(p+1);
    if (prn>MAXPRNGPS) prn+=MINPRNSBS-38;
    if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
        trace(2,"stq subframe satellite number error: prn=%d\n",prn);
        return -1;
    }
    sys=satsys(sat,&prn);
    
    if (sys==SYS_GPS) {
        id=save_subfrm(sat,raw);
        if (id==3) return decode_ephem(sat,raw);
        if (id==4) return decode_alm1 (sat,raw);
        if (id==5) return decode_alm2 (sat,raw);
        return 0;
    }
    return 0;
}
Beispiel #6
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);
}
Beispiel #7
0
/* decode ublox rxm-sfrb: subframe buffer ------------------------------------*/
static int decode_rxmsfrb(raw_t *raw)
{
    unsigned int words[10];
    int i,prn,sat,sys,id;
    unsigned char *p=raw->buff+6;
    
    trace(4,"decode_rxmsfrb: len=%d\n",raw->len);
    
    if (raw->len<42) {
        trace(2,"ubx rxmsfrb length error: len=%d\n",raw->len);
        return -1;
    }
    prn=U1(p+1);
    if (!(sat=satno(MINPRNSBS<=prn?SYS_SBS:SYS_GPS,prn))) {
        trace(2,"ubx rxmsfrb satellite number error: prn=%d\n",prn);
        return -1;
    }
    sys=satsys(sat,&prn);
    
    if (sys==SYS_GPS) {
        id=save_subfrm(sat,raw);
        if (id==3) return decode_ephem(sat,raw);
        if (id==4) return decode_alm1 (sat,raw);
        if (id==5) return decode_alm2 (sat,raw);
        return 0;
    }
    else if (sys==SYS_SBS) {
        for (i=0,p+=2;i<10;i++,p+=4) words[i]=U4(p);
        return sbsdecodemsg(raw->time,prn,words,&raw->sbsmsg)?3:0;
    }
    return 0;
}
Beispiel #8
0
/* average of single position ------------------------------------------------*/
static int avepos(double *ra, int rcv, const obs_t *obs, const nav_t *nav,
                  const prcopt_t *opt)
{
    obsd_t data[MAXOBS];
    gtime_t ts={0};
    sol_t sol={{0}};
    int i,j,n=0,m,iobs;
    char msg[128];
    
    trace(3,"avepos: rcv=%d obs.n=%d\n",rcv,obs->n);
    
    for (i=0;i<3;i++) ra[i]=0.0;
    
    for (iobs=0;(m=nextobsf(obs,&iobs,rcv))>0;iobs+=m) {
        
        for (i=j=0;i<m&&i<MAXOBS;i++) {
            data[j]=obs->data[iobs+i];
            if ((satsys(data[j].sat,NULL)&opt->navsys)&&
                opt->exsats[data[j].sat-1]!=1) j++;
        }
        if (j<=0||!screent(data[0].time,ts,ts,1.0)) continue; /* only 1 hz */
        
        if (!pntpos(data,j,nav,opt,&sol,NULL,NULL,msg)) continue;
        
        for (i=0;i<3;i++) ra[i]+=sol.rr[i];
        n++;
    }
    if (n<=0) {
        trace(1,"no average of base station position\n");
        return 0;
    }
    for (i=0;i<3;i++) ra[i]/=n;
    return 1;
}
Beispiel #9
0
/* long term correction ------------------------------------------------------*/
static int sbslongcorr(gtime_t time, int sat, const sbssat_t *sbssat,
                       double *drs, double *ddts)
{
    const sbssatp_t *p;
    double t;
    int i;
    
    trace(3,"sbslongcorr: sat=%2d\n",sat);
    
    for (p=sbssat->sat;p<sbssat->sat+sbssat->nsat;p++) {
        if (p->sat!=sat||p->lcorr.t0.time==0) continue;
        t=timediff(time,p->lcorr.t0);
        if (fabs(t)>MAXSBSAGEL) {
            trace(2,"sbas long-term correction expired: %s sat=%2d t=%5.0f\n",
                  time_str(time,0),sat,t);
            return 0;
        }
        for (i=0;i<3;i++) drs[i]=p->lcorr.dpos[i]+p->lcorr.dvel[i]*t;
        *ddts=p->lcorr.daf0+p->lcorr.daf1*t;
        
        trace(5,"sbslongcorr: sat=%2d drs=%7.2f%7.2f%7.2f ddts=%7.2f\n",
              sat,drs[0],drs[1],drs[2],*ddts*CLIGHT);
        
        return 1;
    }
    /* if sbas satellite without correction, no correction applied */
    if (satsys(sat,NULL)==SYS_SBS) return 1;
    
    trace(2,"no sbas long-term correction: %s sat=%2d\n",time_str(time,0),sat);
    return 0;
}
Beispiel #10
0
/* psendorange with code bias correction -------------------------------------*/
static double prange(const obsd_t *obs, const nav_t *nav, const double *azel,
                     int iter, const prcopt_t *opt, double *var)
{
    const double *lam=nav->lam[obs->sat-1];
    double PC,P1,P2,P1_P2,P1_C1,P2_C2,gamma;
    int i=0,j=1,sys;
    
    *var=0.0;
    
    if (!(sys=satsys(obs->sat,NULL))) return 0.0;
    
    /* L1-L2 for GPS/GLO/QZS, L1-L5 for GAL/SBS */
    if (NFREQ>=3&&(sys&(SYS_GAL|SYS_SBS))) j=2;
    
    if (NFREQ<2||lam[i]==0.0||lam[j]==0.0) return 0.0;
    
    /* test snr mask */
    if (iter>0) {
        if (testsnr(0,i,azel[1],obs->SNR[i]*0.25,&opt->snrmask)) {
            trace(4,"snr mask: %s sat=%2d el=%.1f snr=%.1f\n",
                  time_str(obs->time,0),obs->sat,azel[1]*R2D,obs->SNR[i]*0.25);
            return 0.0;
        }
        if (opt->ionoopt==IONOOPT_IFLC) {
            if (testsnr(0,j,azel[1],obs->SNR[j]*0.25,&opt->snrmask)) return 0.0;
        }
    }
    gamma=SQR(lam[j])/SQR(lam[i]); /* f1^2/f2^2 */
    P1=obs->P[i];
    P2=obs->P[j];
    P1_P2=nav->cbias[obs->sat-1][0];
    P1_C1=nav->cbias[obs->sat-1][1];
    P2_C2=nav->cbias[obs->sat-1][2];
    
    /* if no P1-P2 DCB, use TGD instead */
    if (P1_P2==0.0&&(sys&(SYS_GPS|SYS_GAL|SYS_QZS))) {
        P1_P2=(1.0-gamma)*gettgd(obs->sat,nav);
    }
    if (opt->ionoopt==IONOOPT_IFLC) { /* dual-frequency */
        
        if (P1==0.0||P2==0.0) return 0.0;
        if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
        if (obs->code[j]==CODE_L2C) P2+=P2_C2; /* C2->P2 */
        
        /* iono-free combination */
        PC=(gamma*P1-P2)/(gamma-1.0);
    }
    else { /* single-frequency */
        
        if (P1==0.0) return 0.0;
        if (obs->code[i]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
        PC=P1-P1_P2/(1.0-gamma);
    }
    if (opt->sateph==EPHOPT_SBAS) PC-=P1_C1; /* sbas clock based C1 */
    
    *var=SQR(ERR_CBIAS);
    
    return PC;
}
Beispiel #11
0
/* average LC ----------------------------------------------------------------*/
static void average_LC(rtk_t *rtk, const obsd_t *obs, int n, const nav_t *nav,
                       const double *azel)
{
    ambc_t *amb;
    double LC1,LC2,LC3,var1,var2,var3,sig;
    int i,j,sat;

    for (i=0; i<n; i++) {
        sat=obs[i].sat;

        if (azel[1+2*i]<rtk->opt.elmin) continue;

        if (satsys(sat,NULL)!=SYS_GPS) continue;

        /* triple-freq carrier and code LC (m) */
        LC1=L_LC(1,-1, 0,obs[i].L)-P_LC(1,1,0,obs[i].P);
        LC2=L_LC(0, 1,-1,obs[i].L)-P_LC(0,1,1,obs[i].P);
        LC3=L_LC(1,-6, 5,obs[i].L)-P_LC(1,1,0,obs[i].P);

        sig=sqrt(SQR(rtk->opt.err[1])+SQR(rtk->opt.err[2]/sin(azel[1+2*i])));

        /* measurement noise variance (m) */
        var1=var_LC(1,1,0,sig*rtk->opt.eratio[0]);
        var2=var_LC(0,1,1,sig*rtk->opt.eratio[0]);
        var3=var_LC(1,1,0,sig*rtk->opt.eratio[0]);

        amb=rtk->ambc+sat-1;

        if (rtk->ssat[sat-1].slip[0]||rtk->ssat[sat-1].slip[1]||
                rtk->ssat[sat-1].slip[2]||amb->n[0]==0.0||
                fabs(timediff(amb->epoch[0],obs[0].time))>MIN_ARC_GAP) {

            amb->n[0]=amb->n[1]=amb->n[2]=0.0;
            amb->LC[0]=amb->LC[1]=amb->LC[2]=0.0;
            amb->LCv[0]=amb->LCv[1]=amb->LCv[2]=0.0;
            amb->fixcnt=0;
            for (j=0; j<MAXSAT; j++) amb->flags[j]=0;
        }
        /* averaging */
        if (LC1) {
            amb->n[0]+=1.0;
            amb->LC [0]+=(LC1 -amb->LC [0])/amb->n[0];
            amb->LCv[0]+=(var1-amb->LCv[0])/amb->n[0];
        }
        if (LC2) {
            amb->n[1]+=1.0;
            amb->LC [1]+=(LC2 -amb->LC [1])/amb->n[1];
            amb->LCv[1]+=(var2-amb->LCv[1])/amb->n[1];
        }
        if (LC3) {
            amb->n[2]+=1.0;
            amb->LC [2]+=(LC3 -amb->LC [2])/amb->n[2];
            amb->LCv[2]+=(var3-amb->LCv[2])/amb->n[2];
        }
        amb->epoch[0]=obs[0].time;
    }
}
/* initialize tracking struct --------------------------------------------------
* set value to tracking struct
* args   : int    sat       I   satellite number
*          int    ctype     I   code type (CTYPE_L1CA...)
*          double ctime     I   code period (s)
*          sdrtrk_t *trk    I/0 tracking struct
* return : int                  0:okay -1:error
*-----------------------------------------------------------------------------*/
extern int inittrkstruct(int sat, int ctype, double ctime, sdrtrk_t *trk)
{
    int i,prn;
    int ctimems=(int)(ctime*1000);
    int sys=satsys(sat,&prn);

    /* set tracking parameter */
    inittrkprmstruct(trk);

    /* correlation point */
    trk->corrn=sdrini.trkcorrn;
    trk->corrp=(int *)malloc(sizeof(int)*trk->corrn);
    for (i=0;i<trk->corrn;i++) {
        trk->corrp[i]=sdrini.trkcorrd*(i+1);
        if (trk->corrp[i]==sdrini.trkcorrp){
            trk->ne=2*(i+1)-1; /* Early */
            trk->nl=2*(i+1);   /* Late */
        }
    }
    /* correlation point for plot */
    (trk->corrx=(double *)calloc(2*trk->corrn+1,sizeof(double)));
    for (i=1;i<=trk->corrn;i++) {
        trk->corrx[2*i-1]=-sdrini.trkcorrd*i;
        trk->corrx[2*i  ]= sdrini.trkcorrd*i;
    }

    trk->II     =(double*)calloc(1+2*trk->corrn,sizeof(double));
    trk->QQ     =(double*)calloc(1+2*trk->corrn,sizeof(double));
    trk->oldI   =(double*)calloc(1+2*trk->corrn,sizeof(double));
    trk->oldQ   =(double*)calloc(1+2*trk->corrn,sizeof(double));
    trk->sumI   =(double*)calloc(1+2*trk->corrn,sizeof(double));
    trk->sumQ   =(double*)calloc(1+2*trk->corrn,sizeof(double));
    trk->oldsumI=(double*)calloc(1+2*trk->corrn,sizeof(double));
    trk->oldsumQ=(double*)calloc(1+2*trk->corrn,sizeof(double));

    if (ctype==CTYPE_L1CA)   trk->loop=LOOP_L1CA;
    if (ctype==CTYPE_G1)     trk->loop=LOOP_G1;
    if (ctype==CTYPE_E1B)    trk->loop=LOOP_E1B;
    if (ctype==CTYPE_L1SAIF) trk->loop=LOOP_SBAS;
    if (ctype==CTYPE_L1SBAS) trk->loop=LOOP_SBAS;
    if (ctype==CTYPE_B1I&&prn>5 ) trk->loop=LOOP_B1I;
    if (ctype==CTYPE_B1I&&prn<=5) trk->loop=LOOP_B1IG;
    
    /* for LEX */
    if (sys==SYS_QZS&&ctype==CTYPE_L1CA&&sdrini.nchL6) trk->loop=LOOP_LEX;
    
    /* loop interval (ms) */
    trk->loopms=trk->loop*ctimems;

    if (!trk->II||!trk->QQ||!trk->oldI||!trk->oldQ||!trk->sumI||!trk->sumQ||
        !trk->oldsumI||!trk->oldsumQ) {
        SDRPRINTF("error: inittrkstruct memory allocation\n");
        return -1;
    }
    return 0;
}
Beispiel #13
0
/* ionosphere and antenna corrected measurements -----------------------------*/
static int corrmeas(const obsd_t *obs, const nav_t *nav, const double *pos,
                    const double *azel, const prcopt_t *opt,
                    const double *dantr, const double *dants, double phw,
                    double *meas, double *var, int *brk)
{
    const double *lam=nav->lam[obs->sat-1];
    double ion=0.0,L1,P1,PC,P1_P2,P1_C1,vari,gamma;
    int i;
    
    trace(4,"corrmeas:\n");
    
    meas[0]=meas[1]=var[0]=var[1]=0.0;
    
    /* iono-free LC */
    if (opt->ionoopt==IONOOPT_IFLC) {
        return ifmeas(obs,nav,azel,opt,dantr,dants,phw,meas,var);
    }
    if (lam[0]==0.0||obs->L[0]==0.0||obs->P[0]==0.0) return 0;
    
    if (testsnr(0,0,azel[1],obs->SNR[0]*0.25,&opt->snrmask)) return 0;
    
    L1=obs->L[0]*lam[0];
    P1=obs->P[0];
    
    /* dcb correction */
    gamma=SQR(lam[1]/lam[0]); /* f1^2/f2^2 */
    P1_P2=nav->cbias[obs->sat-1][0];
    P1_C1=nav->cbias[obs->sat-1][1];
    if (P1_P2==0.0&&(satsys(obs->sat,NULL)&(SYS_GPS|SYS_GAL|SYS_QZS))) {
        P1_P2=(1.0-gamma)*gettgd(obs->sat,nav);
    }
    if (obs->code[0]==CODE_L1C) P1+=P1_C1; /* C1->P1 */
    PC=P1-P1_P2/(1.0-gamma);               /* P1->PC */
    
    /* slant ionospheric delay L1 (m) */
    if (!corr_ion(obs->time,nav,obs->sat,pos,azel,opt->ionoopt,&ion,&vari,brk)) {
        
        trace(2,"iono correction error: time=%s sat=%2d ionoopt=%d\n",
              time_str(obs->time,2),obs->sat,opt->ionoopt);
        return 0;
    }
    /* ionosphere and windup corrected phase and code */
    meas[0]=L1+ion-lam[0]*phw;
    meas[1]=PC-ion;
    
    var[0]+=vari;
    var[1]+=vari+SQR(ERR_CBIAS);
    
    /* antenna phase center variation correction */
    for (i=0;i<2;i++) {
        if (dants) meas[i]-=dants[0];
        if (dantr) meas[i]-=dantr[0];
    }
    return 1;
}
Beispiel #14
0
// get system color ---------------------------------------------------------
TColor __fastcall TPlot::SysColor(int sat)
{
    switch (satsys(sat,NULL)) {
        case SYS_GPS: return MColor[0][1];
        case SYS_GLO: return MColor[0][2];
        case SYS_GAL: return MColor[0][3];
        case SYS_QZS: return MColor[0][4];
        case SYS_CMP: return MColor[0][5];
        case SYS_SBS: return MColor[0][6];
    }
    return MColor[0][0];
}
Beispiel #15
0
/* tle_pos() accuracy --------------------------------------------------------*/
static void utest3(void)
{
    const char *file1="../data/tle/brdc3050.12*";
    const char *file2="../data/tle/TLE_GNSS_20121101.txt";
    const char *file3="../data/tle/igs17127.erp";
    const double ep[6]={2012,10,31,0,0,0};
    nav_t nav={0};
    erp_t erp={0};
    tle_t tle={0};
    gtime_t time;
    char sat[32];
    double rs1[6],rs2[6],ds[6],dts[2],var;
    int i,j,k,stat,svh;
    
    readrnx(file1,0,"",NULL,&nav,NULL);
        assert(nav.n>0);
    
    stat=readerp(file3,&erp);
        assert(stat);
    
    stat=tle_read(file2,&tle);
        assert(stat);
    
    for (i=0;i<MAXSAT;i++) {
        satno2id(i+1,sat);
        
        fprintf(OUT,"SAT=%s\n",sat);
        
        for (j=0;j<96;j++) {
            time=timeadd(epoch2time(ep),900.0*j);
            
            if (!satpos(time,time,i+1,EPHOPT_BRDC,&nav,rs1,dts,&var,&svh)) continue;
            
            if (satsys(i+1,NULL)==SYS_QZS) svh&=0xFE;
            
            if (svh) continue;
            
            stat=tle_pos(time,sat,"","",&tle,&erp,rs2);
                assert(stat);
            
            for (k=0;k<3;k++) ds[k]=rs2[k]-rs1[k];
            
            fprintf(OUT,"%6.0f %11.3f %11.3f %11.3f %11.3f\n",900.0*j,
                    ds[0]/1e3,ds[1]/1e3,ds[2]/1e3,norm(ds,3)/1e3);
                
                assert(norm(ds,3)/1e3<300.0);
        }
        fprintf(OUT,"\n");
    }
    fprintf(OUT,"%s utest3 : OK\n",__FILE__);
}
Beispiel #16
0
/* satellite position and clock by broadcast ephemeris -----------------------*/
static int ephpos(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
                  int iode, double *rs, double *dts, double *var, int *svh)
{
    eph_t  *eph;
    geph_t *geph;
    seph_t *seph;
    double rst[3],dtst[1],tt=1E-3;
    int i,sys;
    
    trace(4,"ephpos  : time=%s sat=%2d iode=%d\n",time_str(time,3),sat,iode);
    
    sys=satsys(sat,NULL);
    
    *svh=-1;
    
    if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
        if (!(eph=seleph(teph,sat,iode,nav))) return 0;
        
        eph2pos(time,eph,rs,dts,var);
        time=timeadd(time,tt);
        eph2pos(time,eph,rst,dtst,var);
        *svh=eph->svh;
    }
    else if (sys==SYS_GLO) {
        if (!(geph=selgeph(teph,sat,iode,nav))) return 0;
        geph2pos(time,geph,rs,dts,var);
        time=timeadd(time,tt);
        geph2pos(time,geph,rst,dtst,var);
        *svh=geph->svh;
    }
    else if (sys==SYS_SBS) {
        if (!(seph=selseph(teph,sat,nav))) return 0;
        
        seph2pos(time,seph,rs,dts,var);
        time=timeadd(time,tt);
        seph2pos(time,seph,rst,dtst,var);
        *svh=seph->svh;
    }
    else return 0;
    
    /* satellite velocity and clock drift by differential approx */
    for (i=0;i<3;i++) rs[i+3]=(rst[i]-rs[i])/tt;
    dts[1]=(dtst[0]-dts[0])/tt;
    
    return 1;
}
Beispiel #17
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");
        }
    }
}
Beispiel #18
0
/* set antenna parameters ----------------------------------------------------*/
static void setpcv(gtime_t time, prcopt_t *popt, nav_t *nav, const pcvs_t *pcvs,
                   const pcvs_t *pcvr, const sta_t *sta)
{
    pcv_t *pcv;
    double pos[3],del[3];
    int i,j,mode=PMODE_DGPS<=popt->mode&&popt->mode<=PMODE_FIXED;
    char id[64];
    
    /* set satellite antenna parameters */
    for (i=0;i<MAXSAT;i++) {
        if (!(satsys(i+1,NULL)&popt->navsys)) continue;
        if (!(pcv=searchpcv(i+1,"",time,pcvs))) {
            satno2id(i+1,id);
            trace(2,"no satellite antenna pcv: %s\n",id);
            continue;
        }
        nav->pcvs[i]=*pcv;
    }
    for (i=0;i<(mode?2:1);i++) {
        if (!strcmp(popt->anttype[i],"*")) { /* set by station parameters */
            strcpy(popt->anttype[i],sta[i].antdes);
            if (sta[i].deltype==1) { /* xyz */
                if (norm(sta[i].pos,3)>0.0) {
                    ecef2pos(sta[i].pos,pos);
                    ecef2enu(pos,sta[i].del,del);
                    for (j=0;j<3;j++) popt->antdel[i][j]=del[j];
                }
            }
            else { /* enu */
                for (j=0;j<3;j++) popt->antdel[i][j]=stas[i].del[j];
            }
        }
        if (!(pcv=searchpcv(0,popt->anttype[i],time,pcvr))) {
            trace(2,"no receiver antenna pcv: %s\n",popt->anttype[i]);
            *popt->anttype[i]='\0';
            continue;
        }
        strcpy(popt->anttype[i],pcv->type);
        popt->pcvr[i]=*pcv;
    }
}
Beispiel #19
0
// update satellite-list pull-down menu -------------------------------------
void __fastcall TPlot::UpdateSatList(void)
{
    int i,j,sys,sysp=0,sat,smask[MAXSAT]={0};
    char s[8];
    
    trace(3,"UpdateSatList\n");
    
    for (i=0;i<2;i++) for (j=0;j<SolStat[i].n;j++) {
        sat=SolStat[i].data[j].sat;
        if (1<=sat&&sat<=MAXSAT) smask[sat-1]=1;
    }
    for (j=0;j<Obs.n;j++) {
        sat=Obs.data[j].sat;
        if (1<=sat&&sat<=MAXSAT) smask[sat-1]=1;
    }
    SatList->Items->Clear();
    SatList->Items->Add("ALL");
    
    for (sat=1;sat<=MAXSAT;sat++) {
        if (SatMask[sat-1]||!smask[sat-1]) continue;
        if ((sys=satsys(sat,NULL))==sysp) continue;
        switch ((sysp=sys)) {
            case SYS_GPS: strcpy(s,"G"); break;
            case SYS_GLO: strcpy(s,"R"); break;
            case SYS_GAL: strcpy(s,"E"); break;
            case SYS_QZS: strcpy(s,"J"); break;
            case SYS_CMP: strcpy(s,"C"); break;
            case SYS_SBS: strcpy(s,"S"); break;
        }
        SatList->Items->Add(s);
    }
    for (sat=1;sat<=MAXSAT;sat++) {
        if (SatMask[sat-1]||!smask[sat-1]) continue;
        satno2id(sat,s);
        SatList->Items->Add(s);
    }
    SatList->ItemIndex=0;
    
    UpdateSatSel();
}
Beispiel #20
0
/* generate rtcm nav data messages -------------------------------------------*/
static void gen_rtcm_nav(gtime_t time, rtcm_t *rtcm, const nav_t *nav,
                         int *index, const int *type, int n, FILE *fp)
{
    int i,j,sat,prn;
    
    for (i=index[0];i<nav->n;i++) {
        
        if (time.time&&timediff(nav->eph[i].ttr,time)>-0.1) continue;
        sat=nav->eph[i].sat;
        rtcm->time=nav->eph[i].ttr;
        rtcm->nav.eph[sat-1]=nav->eph[i];
        rtcm->ephsat=sat;
        
        for (j=0;j<n;j++) {
            if (!is_nav(type[j])) continue;
            
            if (!gen_rtcm3(rtcm,type[j],0)) continue;
            if (fwrite(rtcm->buff,rtcm->nbyte,1,fp)<1) break;
        }
        index[0]=i+1;
    }
    for (i=index[1];i<nav->ng;i++) {
        
        if (time.time&&timediff(nav->geph[i].tof,time)>-0.1) continue;
        sat=nav->geph[i].sat;
        if (satsys(sat,&prn)!=SYS_GLO) continue;
        rtcm->time=nav->geph[i].tof;
        rtcm->nav.geph[prn-1]=nav->geph[i];
        rtcm->ephsat=sat;
        
        for (j=0;j<n;j++) {
            if (!is_gnav(type[j])) continue;
            
            if (!gen_rtcm3(rtcm,type[j],0)) continue;
            if (fwrite(rtcm->buff,rtcm->nbyte,1,fp)<1) break;
        }
        index[1]=i+1;
    }
}
Beispiel #21
0
/* pseudorange residuals -----------------------------------------------------*/
static int rescode(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, prcopt_t *opt,
                   double *v, double *H, double *var, double *azel, int *vsat,
				   double *resp, int *ns, rtk_t *rtk)
{
    double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P;
	int i, j, nv = 0, sys, mask[4] = { 0 }; double azl = 0;
    
    for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3];
    
    ecef2pos(rr,pos);
    
    for (i=*ns=0;i<n&&i<MAXOBS;i++) {
        vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;

		if (obs[i].sat>32){   //added by yuan;
			continue;
		}
		//���������ز�����α��Ͳ�����������ǵķ���;
		if ((fabs(obs[i].L[0]) < 0.0001) || (fabs(obs[i].P[0]) < 0.0001)){
			continue;
		}
        if (!(sys=satsys(obs[i].sat,NULL))) continue;
        
        /* reject duplicated observation data */
        if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
            
            i++;
            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;
		azl = satazel(pos, e, azel + i * 2);
        /* psudorange with code bias correction */
        if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;
        
        /* excluded satellite? */
        if (satexclude(obs[i].sat,svh[i],opt)) continue;
        
        /* ionospheric corrections */
        if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,
                      iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue;
        
        /* tropospheric corrections */
        if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
                      iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
            continue;
        }
        /* pseudorange residual */
        v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);

        /* design matrix */
        for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);
        
        /* time system and receiver bias offset correction */
        if      (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}
        else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}
        else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
        else mask[0]=1;
        
        vsat[i]=1; resp[i]=v[nv]; (*ns)++;
        
		rtk->sat_[nv] = obs[i].sat;  //added by yuan;
		/* error variance */
		if ((rtk->counter > 5) && (obs[i].mark_ < 4)){
			var[nv++] = varerr(opt, azel[1 + i * 2], sys) + (4 - obs[i].mark_)*2 + vare[i] + vmeas + vion + vtrp;  //varm:α���ز�����;vare:������������;vart:����������;
			//var[nv - 1] = var[nv - 1] * (4-obs[i].mark_);    //�������ʧ���������������Ŵ�;
		}
		else{
			var[nv++] = varerr(opt, azel[1 + i * 2], sys) + vare[i] + vmeas + vion + vtrp;  //varm:α���ز�����;vare:������������;vart:����������;
		}
        
        
    }
    /* constraint to avoid rank-deficient */
    for (i=0;i<4;i++) {
        if (mask[i]) continue;
        v[nv]=0.0;
        for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
        var[nv++]=0.01;
    }
    return nv;
}
Beispiel #22
0
/* update rtk server struct --------------------------------------------------*/
static void updatesvr(rtksvr_t *svr, int ret, obs_t *obs, nav_t *nav, int sat,
                      sbsmsg_t *sbsmsg, int index, int iobs)
{
    eph_t *eph1,*eph2,*eph3;
    geph_t *geph1,*geph2,*geph3;
    gtime_t tof;
    double pos[3],del[3],dr[3];
    int i,n=0,prn=0,sbssat=svr->rtk.opt.sbassatsel,sys=0,iode=0;

    INIT_ZERO(del);
    INIT_ZERO(dr);
    INIT_ZERO(pos);
    INIT_ZERO(prn);


    tracet(4,"updatesvr: ret=%d sat=%2d index=%d\n",ret,sat,index);
    
    if (ret==1) { /* observation data */
        if (iobs<MAXOBSBUF) {
            for (i=0;i<obs->n;i++) {
                if (svr->rtk.opt.exsats[obs->data[i].sat-1]==1||
                    !(satsys(obs->data[i].sat,NULL)&svr->rtk.opt.navsys)) continue;
                svr->obs[index][iobs].data[n]=obs->data[i];
                svr->obs[index][iobs].data[n++].rcv=index+1;
            }
            svr->obs[index][iobs].n=n;
            sortobs(&svr->obs[index][iobs]);
        }
        svr->nmsg[index][0]++;
    }
    else if (ret==2) { /* ephemeris */
        if (satsys(sat,&prn)!=SYS_GLO) {
            if (!svr->navsel||svr->navsel==index+1) {
                eph1=nav->eph+sat-1;
                eph2=svr->nav.eph+sat-1;
                eph3=svr->nav.eph+sat-1+MAXSAT;
                if (eph2->ttr.time==0||
                    (eph1->iode!=eph3->iode&&eph1->iode!=eph2->iode)||
                    (timediff(eph1->toe,eph3->toe)!=0.0&&
                     timediff(eph1->toe,eph2->toe)!=0.0)) {
                    *eph3=*eph2;
                    *eph2=*eph1;
                    updatenav(&svr->nav);
                }
            }
            svr->nmsg[index][1]++;
        }
        else {
           if (!svr->navsel||svr->navsel==index+1) {
               geph1=nav->geph+prn-1;
               geph2=svr->nav.geph+prn-1;
               geph3=svr->nav.geph+prn-1+MAXPRNGLO;
               if (geph2->tof.time==0||
                   (geph1->iode!=geph3->iode&&geph1->iode!=geph2->iode)) {
                   *geph3=*geph2;
                   *geph2=*geph1;
                   updatenav(&svr->nav);
                   updatefcn(svr);
               }
           }
           svr->nmsg[index][6]++;
        }
    }
    else if (ret==3) { /* sbas message */
        if (sbsmsg&&(sbssat==sbsmsg->prn||sbssat==0)) {
            if (svr->nsbs<MAXSBSMSG) {
                svr->sbsmsg[svr->nsbs++]=*sbsmsg;
            }
            else {
                for (i=0;i<MAXSBSMSG-1;i++) svr->sbsmsg[i]=svr->sbsmsg[i+1];
                svr->sbsmsg[i]=*sbsmsg;
            }
            sbsupdatecorr(sbsmsg,&svr->nav);
        }
        svr->nmsg[index][3]++;
    }
    else if (ret==9) { /* ion/utc parameters */
        if (svr->navsel==index||svr->navsel>=3) {
            for (i=0;i<8;i++) svr->nav.ion_gps[i]=nav->ion_gps[i];
            for (i=0;i<4;i++) svr->nav.utc_gps[i]=nav->utc_gps[i];
            for (i=0;i<4;i++) svr->nav.ion_gal[i]=nav->ion_gal[i];
            for (i=0;i<4;i++) svr->nav.utc_gal[i]=nav->utc_gal[i];
            for (i=0;i<8;i++) svr->nav.ion_qzs[i]=nav->ion_qzs[i];
            for (i=0;i<4;i++) svr->nav.utc_qzs[i]=nav->utc_qzs[i];
            svr->nav.leaps=nav->leaps;
        }
        svr->nmsg[index][2]++;
    }
    else if (ret==5) { /* antenna postion parameters */
        if (svr->rtk.opt.refpos==4&&index==1) {
            for (i=0;i<3;i++) {
                svr->rtk.rb[i]=svr->rtcm[1].sta.pos[i];
            }
            /* antenna delta */
            ecef2pos(svr->rtk.rb,pos);
            if (svr->rtcm[1].sta.deltype) { /* xyz */
                del[2]=svr->rtcm[1].sta.hgt;
                enu2ecef(pos,del,dr);
                for (i=0;i<3;i++) {
                    svr->rtk.rb[i]+=svr->rtcm[1].sta.del[i]+dr[i];
                }
            }
            else { /* enu */
                enu2ecef(pos,svr->rtcm[1].sta.del,dr);
                for (i=0;i<3;i++) {
                    svr->rtk.rb[i]+=dr[i];
                }
            }
        }
        svr->nmsg[index][4]++;
    }
    else if (ret==7) { /* dgps correction */
        svr->nmsg[index][5]++;
    }
    else if (ret==10) { /* ssr message */
        for (i=0;i<MAXSAT;i++) {
            if (!svr->rtcm[index].ssr[i].update) continue;
            svr->rtcm[index].ssr[i].update=0;
            
            iode=svr->rtcm[index].ssr[i].iode;
            sys=satsys(i+1,&prn);
            
            /* check corresponding ephemeris exists */
            if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS) {
                if (svr->nav.eph[i       ].iode!=iode&&
                    svr->nav.eph[i+MAXSAT].iode!=iode) {
                    continue;
                }
            }
            else if (sys==SYS_GLO) {
                if (svr->nav.geph[prn-1          ].iode!=iode&&
                    svr->nav.geph[prn-1+MAXPRNGLO].iode!=iode) {
                    continue;
                }
            }
            svr->nav.ssr[i]=svr->rtcm[index].ssr[i];
        }
        svr->nmsg[index][7]++;
    }
    else if (ret==31) { /* lex message */
        lexupdatecorr(&svr->raw[index].lexmsg,&svr->nav,&tof);
        svr->nmsg[index][8]++;
    }
    else if (ret==-1) { /* error */
        svr->nmsg[index][9]++;
    }
}
Beispiel #23
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);
}
Beispiel #24
0
/* decode binex mesaage 0x7f-05: trimble netr8 obs data ----------------------*/
static unsigned char *decode_bnx_7f_05_obs(raw_t *raw, unsigned char *buff,
                                           int sat, int nobs, obsd_t *data)
{
    const unsigned char codes_gps[32]={
        CODE_L1C ,CODE_L1C ,CODE_L1P ,CODE_L1W ,CODE_L1Y ,CODE_L1M , /*  0- 5 */
        CODE_L1X ,CODE_L1N ,CODE_NONE,CODE_NONE,CODE_L2W ,CODE_L2C , /*  6-11 */
        CODE_L2D ,CODE_L2S ,CODE_L2L ,CODE_L2X ,CODE_L2P ,CODE_L2W , /* 12-17 */
        CODE_L2Y ,CODE_L2M ,CODE_L2N ,CODE_NONE,CODE_NONE,CODE_L5X , /* 18-23 */
        CODE_L5I ,CODE_L5Q ,CODE_L5X                                 /* 24-26 */
    };
    const unsigned char codes_glo[32]={
        CODE_L1C ,CODE_L1C ,CODE_L1P ,CODE_NONE,CODE_NONE,CODE_NONE, /*  0- 5 */
        CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE,CODE_L2C ,CODE_L2C , /*  6-11 */
        CODE_L2P ,CODE_L3X ,CODE_L3I ,CODE_L3Q ,CODE_L3X             /* 12-16 */
    };
    const unsigned char codes_gal[32]={
        CODE_L1C ,CODE_L1A ,CODE_L1B ,CODE_L1C ,CODE_L1X ,CODE_L1Z , /*  0- 5 */
        CODE_L5X ,CODE_L5I ,CODE_L5Q ,CODE_L5X ,CODE_L7X ,CODE_L7I , /*  6-11 */
        CODE_L7Q ,CODE_L7X ,CODE_L8X ,CODE_L8I ,CODE_L8Q ,CODE_L8X , /* 12-17 */
        CODE_L6X ,CODE_L6A ,CODE_L6B ,CODE_L6C ,CODE_L6X ,CODE_L6Z , /* 18-23 */
    };
    const unsigned char codes_sbs[32]={
        CODE_L1C ,CODE_L1C ,CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE, /*  0- 5 */
        CODE_L5X ,CODE_L5I ,CODE_L5Q ,CODE_L5X                       /*  6- 9 */
    };
    const unsigned char codes_cmp[32]={
        CODE_L2X ,CODE_L2I ,CODE_L2Q ,CODE_L2X ,CODE_L7X ,CODE_L7I , /*  0- 5 */
        CODE_L7Q ,CODE_L7X ,CODE_L6X ,CODE_L6I ,CODE_L6Q ,CODE_L6X , /*  6-11 */
        CODE_L1X ,CODE_L1S ,CODE_L1L ,CODE_L1X                       /* 12-15 */
    };
    const unsigned char codes_qzs[32]={
        CODE_L1C ,CODE_L1C ,CODE_L1S ,CODE_L1L ,CODE_L1X ,CODE_NONE, /*  0- 5 */
        CODE_NONE,CODE_L2X ,CODE_L2S ,CODE_L2L ,CODE_L2X ,CODE_NONE, /*  6-11 */
        CODE_NONE,CODE_L5X ,CODE_L5I ,CODE_L5Q ,CODE_L5X ,CODE_NONE, /* 12-17 */
        CODE_NONE,CODE_L6X ,CODE_L6S ,CODE_L6L ,CODE_L6X ,CODE_NONE, /* 18-23 */
        CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE,CODE_NONE, /* 24-29 */
        CODE_L1Z                                                     /* 30-30 */
    };
    const unsigned char *codes=NULL;
    double range[8],phase[8],cnr[8],dopp[8]={0},acc,wl;
    unsigned char *p=buff;
    unsigned char flag,flag0,flag1,flag2,flag3;
    int i,j,k,sys,fcn=-10,code[8],slip[8],pri[8],freq[8],slipcnt[8]={0},mask[8]={0};
    
    trace(5,"decode_bnx_7f_05_obs: sat=%2d nobs=%2d\n",sat,nobs);
    
    sys=satsys(sat,NULL);
    
    switch (sys) {
        case SYS_GPS: codes=codes_gps; break;
        case SYS_GLO: codes=codes_glo; break;
        case SYS_GAL: codes=codes_gal; break;
        case SYS_QZS: codes=codes_qzs; break;
        case SYS_SBS: codes=codes_sbs; break;
        case SYS_CMP: codes=codes_cmp; break;
    }
    for (i=0;i<nobs;i++) {
        
        flag   =getbitu(p,0,1);
        slip[i]=getbitu(p,2,1);
        code[i]=getbitu(p,3,5); p++;
        
        flag0=flag1=flag2=flag3=0;
        
        if (flag)       flag0=U1(p++);
        if (flag0&0x80) flag1=U1(p++);
        if (flag1&0x80) flag2=U1(p++);
        if (flag2&0x80) flag3=U1(p++);
        if (flag1&0x80) fcn=getbits(&flag2,2,4);
        
        acc=(flag0&0x20)?0.0001:0.00002; /* phase accuracy */
        
        cnr[i]=U1(p++)*0.4;
        
        if (i==0) {
            cnr[i]+=getbits(p,0,2)*0.1;
            range[i]=getbitu(p,2,32)*0.064+getbitu(p,34,6)*0.001; p+=5;
        }
        else if (flag0&0x40) {
            cnr[i]+=getbits(p,0,2)*0.1;
            range[i]=range[0]+getbits(p,4,20)*0.001; p+=3;
        }
        else {
            range[i]=range[0]+getbits(p,0,16)*0.001; p+=2;
        }
        if (flag0&0x40) {
            phase[i]=range[i]+getbits(p,0,24)*acc; p+=3;
        }
        else {
            cnr[i]+=getbits(p,0,2)*0.1;
            phase[i]=range[i]+getbits(p,2,22)*acc; p+=3;
        }
        if (flag0&0x04) {
            dopp[i]=getbits(p,0,24)/256.0; p+=3;
        }
        if (flag0&0x18) {
            slipcnt[i]=U2(p); p+=2;
        }
        else if (flag0&0x08) {
            slipcnt[i]=U1(p); p+=1;
        }
        trace(5,"(%d) CODE=%2d S=%d F=%02X %02X %02X %02X\n",i+1,
              code[i],slip,flag0,flag1,flag2,flag3);
        trace(5,"(%d) P=%13.3f L=%13.3f D=%7.1f SNR=%4.1f SCNT=%2d\n",
              i+1,range[i],phase[i],dopp[i],cnr[i],slipcnt[i]);
    }
    if (!codes) {
        data->sat=0;
        return p;
    }
    data->time=raw->time;
    data->sat=sat;
    
    /* get code priority */
    for (i=0;i<nobs;i++) {
        code2obs(codes[code[i]&0x3F],freq+i);
        pri[i]=getcodepri(sys,codes[code[i]&0x3F],raw->opt);
    }
    for (i=0;i<NFREQ;i++) {
        for (j=0,k=-1;j<nobs;j++) {
            if (freq[j]==i+1&&(k<0||pri[j]>pri[k])) k=j;
        }
        if (k<0) {
            data->P[i]=data->L[i]=0.0;
            data->D[i]=0.0f;
            data->SNR[i]=data->LLI[i]=0;
            data->code[i]=CODE_NONE;
        }
        else {
            wl=satwavelen(sat,i,&raw->nav);
            if (sys==SYS_GLO&&fcn>=-7&&freq[k]<=2) {
                wl=CLIGHT/(freq[k]==1?FREQ1_GLO+DFRQ1_GLO*fcn:
                                      FREQ2_GLO+DFRQ2_GLO*fcn);
            }
            data->P[i]=range[k];
            data->L[i]=wl<=0.0?0.0:phase[k]/wl;
            data->D[i]=dopp[k];
            data->SNR[i]=(unsigned char)(cnr[k]/0.25+0.5);
            data->code[i]=codes[code[k]&0x3F];
            data->LLI[i]=slip[k]?1:0;
            mask[k]=1;
        }
    }
    for (;i<NFREQ+NEXOBS;i++) {
        for (k=0;k<nobs;k++) {
            if (!mask[k]) break;
        }
        if (k>=nobs) {
            data->P[i]=data->L[i]=0.0;
            data->D[i]=0.0f;
            data->SNR[i]=data->LLI[i]=0;
            data->code[i]=CODE_NONE;
        }
        else {
            wl=satwavelen(sat,freq[k]-1,&raw->nav);
            if (sys==SYS_GLO&&fcn>=-7&&freq[k]<=2) {
                wl=CLIGHT/(freq[k]==1?FREQ1_GLO+DFRQ1_GLO*fcn:
                                      FREQ2_GLO+DFRQ2_GLO*fcn);
            }
            data->P[i]=range[k];
            data->L[i]=wl<=0.0?0.0:phase[k]/wl;
            data->D[i]=dopp[k];
            data->SNR[i]=(unsigned char)(cnr[k]/0.25+0.5);
            data->code[i]=codes[code[k]&0x3F];
            data->LLI[i]=slip[k]?1:0;
            mask[k]=1;
        }
    }
    return p;
}
Beispiel #25
0
/* initialize receiver raw data control ----------------------------------------
* initialize receiver raw data control struct and reallocate obsevation and
* epheris buffer
* args   : raw_t  *raw   IO     receiver raw data control struct
* return : status (1:ok,0:memory allocation error)
*-----------------------------------------------------------------------------*/
extern int init_raw(raw_t *raw)
{
    const double lam_glo[NFREQ]= {CLIGHT/FREQ1_GLO,CLIGHT/FREQ2_GLO};
    gtime_t time0= {0};
    obsd_t data0= {{0}};
    eph_t  eph0 = {0,-1,-1};
    alm_t  alm0 = {0,-1};
    geph_t geph0= {0,-1};
    seph_t seph0= {0};
    sbsmsg_t sbsmsg0= {0};
    lexmsg_t lexmsg0= {0};
    int i,j,sys;

    trace(3,"init_raw:\n");

    raw->time=raw->tobs=time0;
    raw->ephsat=0;
    raw->sbsmsg=sbsmsg0;
    raw->msgtype[0]='\0';
    for (i=0; i<MAXSAT; i++) {
        for (j=0; j<150  ; j++) raw->subfrm[i][j]=0;
        for (j=0; j<NFREQ; j++) raw->lockt[i][j]=0.0;
        for (j=0; j<NFREQ; j++) raw->halfc[i][j]=0;
        raw->icpp[i]=raw->off[i]=raw->prCA[i]=raw->dpCA[i]=0.0;
    }
    for (i=0; i<MAXOBS; i++) raw->freqn[i]=0;
    raw->lexmsg=lexmsg0;
    raw->icpc=0.0;
    raw->nbyte=raw->len=0;
    raw->iod=raw->flag=raw->tbase=raw->outtype=0;
    raw->tod=-1;
    for (i=0; i<MAXRAWLEN; i++) raw->buff[i]=0;
    raw->opt[0]='\0';

    free_raw(raw);

    if (!(raw->obs.data =(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))||
            !(raw->obuf.data=(obsd_t *)malloc(sizeof(obsd_t)*MAXOBS))||
            !(raw->nav.eph  =(eph_t  *)malloc(sizeof(eph_t )*MAXSAT))||
            !(raw->nav.alm  =(alm_t  *)malloc(sizeof(alm_t )*MAXSAT))||
            !(raw->nav.geph =(geph_t *)malloc(sizeof(geph_t)*NSATGLO))||
            !(raw->nav.seph =(seph_t *)malloc(sizeof(seph_t)*NSATSBS*2))) {
        free_raw(raw);
        return 0;
    }
    raw->obs.n =0;
    raw->obuf.n=0;
    raw->nav.n =MAXSAT;
    raw->nav.na=MAXSAT;
    raw->nav.ng=NSATGLO;
    raw->nav.ns=NSATSBS*2;
    for (i=0; i<MAXOBS   ; i++) raw->obs.data [i]=data0;
    for (i=0; i<MAXOBS   ; i++) raw->obuf.data[i]=data0;
    for (i=0; i<MAXSAT   ; i++) raw->nav.eph  [i]=eph0;
    for (i=0; i<MAXSAT   ; i++) raw->nav.alm  [i]=alm0;
    for (i=0; i<NSATGLO  ; i++) raw->nav.geph [i]=geph0;
    for (i=0; i<NSATSBS*2; i++) raw->nav.seph [i]=seph0;
    for (i=0; i<MAXSAT; i++) for (j=0; j<NFREQ; j++) {
            if (!(sys=satsys(i+1,NULL))) continue;
            raw->nav.lam[i][j]=sys==SYS_GLO?lam_glo[j]:lam[j];
        }
    return 1;
}
Beispiel #26
0
/* broadcast ephemeris to satellite position and clock bias --------------------
* compute satellite position and clock bias with broadcast ephemeris (gps,
* galileo, qzss)
* args   : gtime_t time     I   time (gpst)
*          eph_t *eph       I   broadcast ephemeris
*          double *rs       O   satellite position (ecef) {x,y,z} (m)
*          double *dts      O   satellite clock bias (s)
*          double *var      O   satellite position and clock variance (m^2)
* return : none
* notes  : see ref [1],[7],[8]
*          satellite clock includes relativity correction without code bias
*          (tgd or bgd)
*-----------------------------------------------------------------------------*/
extern void eph2pos(gtime_t time, const eph_t *eph, double *rs, double *dts,
                    double *var)
{
    double tk,M,E,Ek,sinE,cosE,u,r,i,O,sin2u,cos2u,x,y,sinO,cosO,cosi,mu,omge;
    double xg,yg,zg,sino,coso;
    int n,sys,prn;
    
    trace(4,"eph2pos : time=%s sat=%2d\n",time_str(time,3),eph->sat);
    
    if (eph->A<=0.0) {
        rs[0]=rs[1]=rs[2]=*dts=*var=0.0;
        return;
    }
    tk=timediff(time,eph->toe);
    
    switch ((sys=satsys(eph->sat,&prn))) {
        case SYS_GAL: mu=MU_GAL; omge=OMGE_GAL; break;
        case SYS_CMP: mu=MU_CMP; omge=OMGE_CMP; break;
        default:      mu=MU_GPS; omge=OMGE;     break;
    }
    M=eph->M0+(sqrt(mu/(eph->A*eph->A*eph->A))+eph->deln)*tk;
    
    for (n=0,E=M,Ek=0.0;fabs(E-Ek)>RTOL_KEPLER&&n<MAX_ITER_KEPLER;n++) {
        Ek=E; E-=(E-eph->e*sin(E)-M)/(1.0-eph->e*cos(E));
    }
    if (n>=MAX_ITER_KEPLER) {
        trace(2,"kepler iteration overflow sat=%2d\n",eph->sat);
        return;
    }
    sinE=sin(E); cosE=cos(E);
    
    trace(4,"kepler: sat=%2d e=%8.5f n=%2d del=%10.3e\n",eph->sat,eph->e,n,E-Ek);
    
    u=atan2(sqrt(1.0-eph->e*eph->e)*sinE,cosE-eph->e)+eph->omg;
    r=eph->A*(1.0-eph->e*cosE);
    i=eph->i0+eph->idot*tk;
    sin2u=sin(2.0*u); cos2u=cos(2.0*u);
    u+=eph->cus*sin2u+eph->cuc*cos2u;
    r+=eph->crs*sin2u+eph->crc*cos2u;
    i+=eph->cis*sin2u+eph->cic*cos2u;
    x=r*cos(u); y=r*sin(u); cosi=cos(i);
    
    /* beidou geo satellite (ref [9]) */
    if (sys==SYS_CMP&&prn<=5) {
        O=eph->OMG0+eph->OMGd*tk-omge*eph->toes;
        sinO=sin(O); cosO=cos(O);
        xg=x*cosO-y*cosi*sinO;
        yg=x*sinO+y*cosi*cosO;
        zg=y*sin(i);
        sino=sin(omge*tk); coso=cos(omge*tk);
        rs[0]= xg*coso+yg*sino*COS_5+zg*sino*SIN_5;
        rs[1]=-xg*sino+yg*coso*COS_5+zg*coso*SIN_5;
        rs[2]=-yg*SIN_5+zg*COS_5;
    }
    else {
        O=eph->OMG0+(eph->OMGd-omge)*tk-omge*eph->toes;
        sinO=sin(O); cosO=cos(O);
        rs[0]=x*cosO-y*cosi*sinO;
        rs[1]=x*sinO+y*cosi*cosO;
        rs[2]=y*sin(i);
    }
    tk=timediff(time,eph->toc);
    *dts=eph->f0+eph->f1*tk+eph->f2*tk*tk;
    
    /* relativity correction */
    *dts-=2.0*sqrt(mu*eph->A)*eph->e*sinE/SQR(CLIGHT);
    
    /* position and clock error variance */
    *var=var_uraeph(eph->sva);
}
Beispiel #27
0
/* convert nav message -------------------------------------------------------*/
static void convnav(FILE **ofp, rnxopt_t *opt, strfile_t *str, int *n)
{
    gtime_t ts1,te1,ts2,te2;
    int sys,prn;
    
    trace(3,"convnav :\n");
    
    ts1=opt->ts; if (ts1.time!=0) ts1=timeadd(ts1,-MAXDTOE);
    te1=opt->te; if (te1.time!=0) te1=timeadd(te1, MAXDTOE);
    ts2=opt->ts; if (ts2.time!=0) ts2=timeadd(ts2,-MAXDTOE_GLO);
    te2=opt->te; if (te2.time!=0) te2=timeadd(te2, MAXDTOE_GLO);
    
    sys=satsys(str->sat,&prn)&opt->navsys;
    
    if (sys==SYS_GPS) {
        
        if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
        
        if (ofp[1]) {
            
            /* output rinex nav */
            outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
            n[1]++;
        }
    }
    else if (sys==SYS_GLO) {
        
        if (opt->exsats[str->sat-1]==1||!screent(str->time,ts2,te2,0.0)) return;
        
        if (ofp[1]&&opt->rnxver>2.99) {
            
            /* output rinex nav */
            outrnxgnavb(ofp[1],opt,str->nav->geph+prn-1);
            n[1]++;
        }
        if (ofp[2]&&opt->rnxver<=2.99) {
            
            /* output rinex gnav */
            outrnxgnavb(ofp[2],opt,str->nav->geph+prn-1);
            n[2]++;
        }
    }
    else if (sys==SYS_SBS) {
        
        if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
        
        if (ofp[1]&&opt->rnxver>2.99) {
            
            /* output rinex nav */
            outrnxhnavb(ofp[1],opt,str->nav->seph+prn-MINPRNSBS);
            n[1]++;
        }
        if (ofp[3]&&opt->rnxver<=2.99) {
            
            /* output rinex hnav */
            outrnxhnavb(ofp[3],opt,str->nav->seph+prn-MINPRNSBS);
            n[3]++;
        }
    }
    else if (sys==SYS_QZS) {
        
        if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
        
        if (ofp[1]&&opt->rnxver>2.99) {
            
            /* output rinex nav */
            outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
            n[1]++;
        }
        if (ofp[4]&&opt->rnxver<=2.99) {
            
            /* output rinex qnav */
            outrnxnavb(ofp[4],opt,str->nav->eph+str->sat-1);
            n[4]++;
        }
    }
    else if (sys==SYS_GAL) {
        if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
        
        if (ofp[1]&&opt->rnxver>2.99) {
            
            /* output rinex nav */
            outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
            n[1]++;
        }
        if (ofp[5]&&opt->rnxver<=2.99) {
            
            /* output rinex lnav */
            outrnxnavb(ofp[5],opt,str->nav->eph+str->sat-1);
            n[5]++;
        }
    }
    else if (sys==SYS_CMP) {
        if (opt->exsats[str->sat-1]==1||!screent(str->time,ts1,te1,0.0)) return;
        
        if (ofp[1]&&opt->rnxver>2.99) {
            
            /* output rinex nav */
            outrnxnavb(ofp[1],opt,str->nav->eph+str->sat-1);
            n[1]++;
        }
    }
}
Beispiel #28
0
/* scan observation types ----------------------------------------------------*/
static int scan_obstype(int format, const char *file, rnxopt_t *opt,
                        gtime_t *time)
{
    strfile_t *str;
    unsigned char codes[6][33]={{0}};
    unsigned char types[6][33]={{0}};
    char msg[128];
    int i,j,k,l,c=0,type,sys,abort=0,n[6]={0};
    
    trace(3,"scan_obstype: file=%s, opt=%s\n",file,opt);
    
    if (!(str=gen_strfile(format,opt->rcvopt,*time))) return 0;
    
    if (!open_strfile(str,file)) {
        free_strfile(str);
        return 0;
    }
    /* scan codes in input file */
    while ((type=input_strfile(str))>=-1) {
        
        if (type!=1||str->obs->n<=0) continue;
        
        if (!opt->ts.time||timediff(str->obs->data[0].time,opt->ts)>=0.001) {
             
            for (i=0;i<str->obs->n;i++) {
                sys=satsys(str->obs->data[i].sat,NULL);
                for (l=0;navsys[l];l++) if (navsys[l]==sys) break;
                if (!navsys[l]) continue;
                
                for (j=0;j<NFREQ+NEXOBS;j++) {
                    if (!str->obs->data[i].code[j]) continue;
                    
                    for (k=0;k<n[l];k++) {
                        if (codes[l][k]==str->obs->data[i].code[j]) break;
                    }
                    if (k>=n[l]&&n[l]<32) {
                        codes[l][n[l]++]=str->obs->data[i].code[j];
                    }
                    if (k<n[l]) {
                        if (str->obs->data[i].P[j]!=0.0) types[l][k]|=1;
                        if (str->obs->data[i].L[j]!=0.0) types[l][k]|=2;
                        if (str->obs->data[i].D[j]!=0.0) types[l][k]|=4;
                        if (str->obs->data[i].SNR[j]!=0) types[l][k]|=8;
                    }
                }
            }
            if (!time->time) *time=str->obs->data[0].time;
        }
        if (opt->te.time&&timediff(str->obs->data[0].time,opt->te)>10.0) break;
        
        if (++c%11) continue;
        
        sprintf(msg,"scanning: %s %s%s%s%s%s%s",time_str(str->time,0),
                n[0]?"G":"",n[1]?"R":"",n[2]?"E":"",n[3]?"J":"",
                n[4]?"S":"",n[5]?"C":"");
        if ((abort=showmsg(msg))) break;
    }
    showmsg("");
    
    close_strfile(str);
    free_strfile(str);
    
    if (abort) {
        trace(2,"aborted in scan\n");
        return 0;
    }
    for (i=0;i<6;i++) for (j=0;j<n[i];j++) {
        trace(2,"scan_obstype: sys=%d code=%s type=%d\n",i,code2obs(codes[i][j],NULL),types[i][j]);
    }
    for (i=0;i<6;i++) {
        
        /* sort codes */
        sort_codes(codes[i],types[i],n[i]);
        
        /* set observation types in rinex option */
        setopt_obstype(codes[i],types[i],i,opt);
        
        for (j=0;j<n[i];j++) {
            trace(3,"scan_obstype: sys=%d code=%s\n",i,code2obs(codes[i][j],NULL));
        }
    }
    return 1;
}
Beispiel #29
0
/* pseudorange residuals -----------------------------------------------------*/
static int rescode(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, const prcopt_t *opt,
                   double *v, double *H, double *var, double *azel, int *vsat,
                   double *resp, int *ns)
{
    double r,dion,dtrp,vmeas,vion,vtrp,rr[3],pos[3],dtr,e[3],P,lam_L1;
    int i,j,nv=0,sys,mask[4]={0};
    
    trace(3,"resprng : n=%d\n",n);
    
    for (i=0;i<3;i++) rr[i]=x[i]; dtr=x[3];
    
    ecef2pos(rr,pos);
    
    for (i=*ns=0;i<n&&i<MAXOBS;i++) {
        vsat[i]=0; azel[i*2]=azel[1+i*2]=resp[i]=0.0;
        
        if (!(sys=satsys(obs[i].sat,NULL))) continue;
        
        /* reject duplicated observation data */
        if (i<n-1&&i<MAXOBS-1&&obs[i].sat==obs[i+1].sat) {
            trace(2,"duplicated observation data %s sat=%2d\n",
                  time_str(obs[i].time,3),obs[i].sat);
            i++;
            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;
        
        /* psudorange with code bias correction */
        if ((P=prange(obs+i,nav,azel+i*2,iter,opt,&vmeas))==0.0) continue;
        
        /* excluded satellite? */
        if (satexclude(obs[i].sat,svh[i],opt)) continue;
        
        /* ionospheric corrections */
        if (!ionocorr(obs[i].time,nav,obs[i].sat,pos,azel+i*2,
                      iter>0?opt->ionoopt:IONOOPT_BRDC,&dion,&vion)) continue;
        
        /* GPS-L1 -> L1/B1 */
        if ((lam_L1=nav->lam[obs[i].sat-1][0])>0.0) {
            dion*=SQR(lam_L1/lam_carr[0]);
        }
        /* tropospheric corrections */
        if (!tropcorr(obs[i].time,nav,pos,azel+i*2,
                      iter>0?opt->tropopt:TROPOPT_SAAS,&dtrp,&vtrp)) {
            continue;
        }
        /* pseudorange residual */
        v[nv]=P-(r+dtr-CLIGHT*dts[i*2]+dion+dtrp);
        
        /* design matrix */
        for (j=0;j<NX;j++) H[j+nv*NX]=j<3?-e[j]:(j==3?1.0:0.0);
        
        /* time system and receiver bias offset correction */
        if      (sys==SYS_GLO) {v[nv]-=x[4]; H[4+nv*NX]=1.0; mask[1]=1;}
        else if (sys==SYS_GAL) {v[nv]-=x[5]; H[5+nv*NX]=1.0; mask[2]=1;}
        else if (sys==SYS_CMP) {v[nv]-=x[6]; H[6+nv*NX]=1.0; mask[3]=1;}
        else mask[0]=1;
        
        vsat[i]=1; resp[i]=v[nv]; (*ns)++;
        
        /* error variance */
        var[nv++]=varerr(opt,azel[1+i*2],sys)+vare[i]+vmeas+vion+vtrp;
        
        trace(4,"sat=%2d azel=%5.1f %4.1f res=%7.3f sig=%5.3f\n",obs[i].sat,
              azel[i*2]*R2D,azel[1+i*2]*R2D,resp[i],sqrt(var[nv-1]));
    }
    /* constraint to avoid rank-deficient */
    for (i=0;i<4;i++) {
        if (mask[i]) continue;
        v[nv]=0.0;
        for (j=0;j<NX;j++) H[j+nv*NX]=j==i+3?1.0:0.0;
        var[nv++]=0.01;
    }
    return nv;
}
Beispiel #30
0
/* satellite position and clock with ssr correction --------------------------*/
static int satpos_ssr(gtime_t time, gtime_t teph, int sat, const nav_t *nav,
                      int opt, double *rs, double *dts, double *var, int *svh)
{
    const ssr_t *ssr;
    eph_t *eph;
    double t1,t2,t3,er[3],ea[3],ec[3],rc[3],deph[3],dclk,dant[3]={0},tk;
    int i,sys;
    
    trace(4,"satpos_ssr: time=%s sat=%2d\n",time_str(time,3),sat);
    
    ssr=nav->ssr+sat-1;
    
    if (!ssr->t0[0].time) {
        trace(2,"no ssr orbit correction: %s sat=%2d\n",time_str(time,0),sat);
        return 0;
    }
    if (!ssr->t0[1].time) {
        trace(2,"no ssr clock correction: %s sat=%2d\n",time_str(time,0),sat);
        return 0;
    }
    /* inconsistency between orbit and clock correction */
    if (ssr->iod[0]!=ssr->iod[1]) {
        trace(2,"inconsist ssr correction: %s sat=%2d iod=%d %d\n",
              time_str(time,0),sat,ssr->iod[0],ssr->iod[1]);
        *svh=-1;
        return 0;
    }
    t1=timediff(time,ssr->t0[0]);
    t2=timediff(time,ssr->t0[1]);
    t3=timediff(time,ssr->t0[2]);
    
    /* ssr orbit and clock correction (ref [4]) */
    if (fabs(t1)>MAXAGESSR||fabs(t2)>MAXAGESSR) {
        trace(2,"age of ssr error: %s sat=%2d t=%.0f %.0f\n",time_str(time,0),
              sat,t1,t2);
        *svh=-1;
        return 0;
    }
    if (ssr->udi[0]>=1.0) t1-=ssr->udi[0]/2.0;
    if (ssr->udi[1]>=1.0) t2-=ssr->udi[0]/2.0;
    
    for (i=0;i<3;i++) deph[i]=ssr->deph[i]+ssr->ddeph[i]*t1;
    dclk=ssr->dclk[0]+ssr->dclk[1]*t2+ssr->dclk[2]*t2*t2;
    
    /* ssr highrate clock correction (ref [4]) */
    if (ssr->iod[0]==ssr->iod[2]&&ssr->t0[2].time&&fabs(t3)<MAXAGESSR_HRCLK) {
        dclk+=ssr->hrclk;
    }
    if (norm(deph,3)>MAXECORSSR||fabs(dclk)>MAXCCORSSR) {
        trace(3,"invalid ssr correction: %s deph=%.1f dclk=%.1f\n",
              time_str(time,0),norm(deph,3),dclk);
        *svh=-1;
        return 0;
    }
    /* satellite postion and clock by broadcast ephemeris */
    if (!ephpos(time,teph,sat,nav,ssr->iode,rs,dts,var,svh)) return 0;
    
    /* satellite clock for gps, galileo and qzss */
    sys=satsys(sat,NULL);
    if (sys==SYS_GPS||sys==SYS_GAL||sys==SYS_QZS||sys==SYS_CMP) {
        if (!(eph=seleph(teph,sat,ssr->iode,nav))) return 0;
        
        /* satellite clock by clock parameters */
        tk=timediff(time,eph->toc);
        dts[0]=eph->f0+eph->f1*tk+eph->f2*tk*tk;
        dts[1]=eph->f1+2.0*eph->f2*tk;
        
        /* relativity correction */
        dts[0]-=2.0*dot(rs,rs+3,3)/CLIGHT/CLIGHT;
    }
    /* radial-along-cross directions in ecef */
    if (!normv3(rs+3,ea)) return 0;
    cross3(rs,rs+3,rc);
    if (!normv3(rc,ec)) {
        *svh=-1;
        return 0;
    }
    cross3(ea,ec,er);
    
    /* satellite antenna offset correction */
    if (opt) {
        satantoff(time,rs,sat,nav,dant);
    }
    for (i=0;i<3;i++) {
        rs[i]+=-(er[i]*deph[0]+ea[i]*deph[1]+ec[i]*deph[2])+dant[i];
    }
    /* t_corr = t_sv - (dts(brdc) + dclk(ssr) / CLIGHT) (ref [10] eq.3.12-7) */
    dts[0]+=dclk/CLIGHT;
    
    /* variance by ssr ura */
    *var=var_urassr(ssr->ura);
    
    trace(5,"satpos_ssr: %s sat=%2d deph=%6.3f %6.3f %6.3f er=%6.3f %6.3f %6.3f dclk=%6.3f var=%6.3f\n",
          time_str(time,2),sat,deph[0],deph[1],deph[2],er[0],er[1],er[2],dclk,*var);
    
    return 1;
}