ignav中数据流时间对准

 

各数据流时间对准用到的结构体为：

typedef struct {         /* time synchronization index in buffer */
    int ni,nip,imu;      /* current/precious number and sync index of imu measurement data  */
    int nr,nrp,rover;    /* current/precious number and sync index of observation data */
    int ns,nsp,pvt;      /* current/precious number and sync index of pvt solution data */
    int nb,nbp,base;     /* current/precious number and sync index of base observation data */
    int nm,nmp,img;      /* current/precious number and sync index of image raw data */
    int np,npp,pose;     /* current/precious number and sync index of pose measurement */
    int of[6];           /* overflow flag (rover,base,imu,pvt,image,pose) */
    unsigned int tali[6];/* time alignment for data (rover-base,pvt-imu,rover-base-imu,imu-image,pose-imu) */
    unsigned int ws;     /* search window size */
    double dt[6];        /* time difference between input streams */
    gtime_t time[6];     /* current time of rover,base,imu,pvt,image and pose measurement */
} syn_t;

所以，搞清楚结构体中各变量的含义即可。

 

ni、nr、ns、nb、nm、np：表示此次数据流读取后数据历元数

    /* update time synchronization index */
    switch (index) {
        case 0: svr->syn.nr=fobs; break;
        case 1: svr->syn.nb=fobs; break;
        case 3: svr->syn.ns=fobs; break;
        case 4: svr->syn.ni=fobs; break;
        case 5: svr->syn.nm=fobs; break;
        case 6: svr->syn.np=fobs; break;
    }

 

nip、nrp、nsp、nbp、nmp、npp：表示此次数据流读取前数据历元数

    switch (index) {
        case 0: fobs=svr->syn.nr; svr->syn.nrp=svr->syn.nr; break;
        case 1: fobs=svr->syn.nb; svr->syn.nbp=svr->syn.nb; break;
        case 3: fobs=svr->syn.ns; svr->syn.nsp=svr->syn.ns; break;
        case 4: fobs=svr->syn.ni; svr->syn.nip=svr->syn.ni; break;
        case 5: fobs=svr->syn.nm; svr->syn.nmp=svr->syn.nm; break;
        case 6: fobs=svr->syn.np; svr->syn.npp=svr->syn.np; break;
    }

 

imu、rover、pvt、base、img、pose：表示数据处理索引号

tali：0 表示没对准；1 表示时间已对准

注：从代码中可以看出两个宏定义 （DTTOL、DTTOLM）分别用来判断imu与rover、rover与base间数据时间是否对准

　　因为程序设置的为固定值，所以当数据频率有变化时需要对其进行调整（程序默认的为：imu 200HZ，rover和base 1HZ）

/* time alignment for observation and imu data-------------------------------*/
static int imuobsalign(rtksvr_t *svr)
{
    int i,j,k,n; double sow1,sow2,sow3;
    obs_t *p1=NULL,*p2=NULL;
    syn_t *psyn=&svr->syn;

    /* observation and imu data time alignment */
    n=psyn->of[2]?MAXIMUBUF:psyn->ni;
    p1=svr->obs[0];
    p2=svr->obs[1];

    for (i=0;i<n&&svr->syn.tali[2]!=2;i++) { /* start time alignment */

        sow1=time2gpst(svr->imu[i].time,NULL);

        /* match rover observation */
        for (j=0;j<(psyn->of[0]?MAXOBSBUF:psyn->nr);j++) {
            sow2=time2gpst(p1[j].data[0].time,NULL);
            if (p1[j].n) {
                if (fabs(sow1-sow2)>DTTOL) continue;
            }
            psyn->imu    =i;
            psyn->rover  =j;
            psyn->tali[2]=1;
            break;
        }
        /* match base observation */
        if (psyn->tali[2]==1) {
            for (k=0;k<(psyn->of[1]?MAXOBSBUF:psyn->nb);k++) {
                sow3=time2gpst(p2[k].data[0].time,NULL);
                if (p2[k].n) {
                    if (fabs(sow2-sow3)>DTTOLM) continue;
                }
                else continue;
                psyn->base   =k;
                psyn->tali[2]=2;
                break;
            }
        }
        if (psyn->tali[2]==2) {
            tracet(3,"imu and rover/base align ok\n");
            return 1;
        }
        else psyn->tali[2]=0; /* fail */
    }
    return 0;
}

 

time：数据流的时间（当前储存的最新数据的时间）

    /* update current time of measurement */
    if (fobs) {
        if (index==0) svr->syn.time[0]=svr->obs[0][(fobs-1)%MAXOBSBUF].data[0].time;
        if (index==1) svr->syn.time[1]=svr->obs[1][(fobs-1)%MAXOBSBUF].data[0].time;
        if (index==3) svr->syn.time[3]=svr->pvt[(fobs-1)%MAXSOLBUF].time;
        if (index==4) svr->syn.time[2]=svr->imu[(fobs-1)%MAXIMUBUF].time;
        if (index==5) svr->syn.time[4]=svr->img[(fobs-1)%MAXIMGBUF].time;
        if (index==6) svr->syn.time[5]=svr->pose[(fobs-1)%MAXPOSEBUF].time;
    }

 

dt：time间作差，及各数据流时间之差（用来控制个传感器数据读取）

/* update time difference between input stream--------------------------------*/
static void updatetimediff(rtksvr_t *svr)
{
    syn_t *syn=&svr->syn;
    
    trace(3,"updatetimediff:\n");

    if (svr->rtk.opt.mode<=PMODE_FIXED) { /* for gnss position mode */
        if (syn->nr&&syn->nb) {
            syn->dt[0]=time2gpst(syn->time[0],NULL)-time2gpst(syn->time[1],NULL);
        }
    }
    /* ins loosely coupled position mode */
    if (svr->rtk.opt.mode==PMODE_INS_LGNSS) {

        if (svr->rtk.opt.insopt.lcopt==IGCOM_USESOL&&syn->ni&&syn->ns) {
            syn->dt[0]=time2gpst(syn->time[2],NULL)-time2gpst(syn->time[3],NULL);
        }
        if (svr->rtk.opt.insopt.lcopt==IGCOM_USEOBS&&syn->ni) {
            if (syn->nr) {
                syn->dt[0]=time2gpst(syn->time[2],NULL)-time2gpst(syn->time[0],NULL);
            }
            if (syn->nb) {
                syn->dt[1]=time2gpst(syn->time[2],NULL)-time2gpst(syn->time[1],NULL);
            }
        }
        if (svr->rtk.opt.insopt.pose_aid||svr->rtk.opt.insopt.align_dualants) {
            if (syn->np&&syn->ni) {
                syn->dt[2]=time2gpst(syn->time[5],NULL)-
                           time2gpst(syn->time[2],NULL);
            }
        }
    }
    /* ins tightly coupled position mode */
    if (svr->rtk.opt.mode==PMODE_INS_TGNSS) {
        if (syn->ni&&syn->nr) {
            syn->dt[0]=time2gpst(syn->time[2],NULL)-
                       time2gpst(syn->time[0],NULL);
        }
        if (syn->ni&&syn->nb) {
            syn->dt[1]=time2gpst(syn->time[2],NULL)-
                       time2gpst(syn->time[1],NULL);
        }
        if (svr->rtk.opt.insopt.pose_aid||svr->rtk.opt.insopt.align_dualants) {
            if (syn->np&&syn->ni) {
                syn->dt[2]=time2gpst(syn->time[5],NULL)-
                           time2gpst(syn->time[2],NULL);
            }
        }
    }
    /* camera measurement data aid */
    if (syn->nm&&syn->ni) {
        syn->dt[2]=time2gpst(syn->time[2],NULL)-
                   time2gpst(syn->time[4],NULL);
    }
    /* visual odometry and ins loosely coupled */
    if (svr->rtk.opt.mode==PMODE_INS_LVO) {
        syn->dt[0]=time2gpst(syn->time[2],NULL)-
                   time2gpst(syn->time[4],NULL);
    }
}

 suspend()：通过 dt 控制各传感器数据流时间差在 MAXTIMEDIFF 以内，如果超出 MAXTIMEDIFF 就返回flase 读取数据

以rover、imu为例：

imu与rover数据流间的时间差为：syn->dt[0] = syn->time[2] - syn->time[0]（imu数据流时间减去rover数据流时间）

当syn->dt[0]大于零时，说明imu数据流较快；当syn->dt[0]小于零时，说明rover数据流较快。

当 syn->dt[0] > dT 时，一定不满足 syn->dt[0] < -dT，所以会读取 rover 数据流中的数据；

当 syn->dt[0] < -dT 时，一定不满足 syn->dt[0] > dT，所以会读取 imu 数据流中的数据；

当 -dT < syn->dt[0] < dT 时，既不满足一定不满足syn->dt[0] > dT，又不满足 syn->dt[0] < -dT，所以会读取 rover 和imu 数据流中的数据。

所以每次数据读取至少会读取一个传感器数据流中的数据。

/* update input stream supend flag--------------------------------------------*/
static int suspend(rtksvr_t *svr,int index)
{
    syn_t *syn=&svr->syn;
    int lc=svr->rtk.opt.insopt.lcopt;
    double d,dT=MAXTIMEDIFF;

    d=svr->rtk.opt.soltype?-1.0:1.0;

    if (svr->rtk.opt.mode<=PMODE_FIXED) { /* gnss position mode */
        switch (index) {
            case 0: if (d*syn->dt[0]> dT) return 1; break;
            case 1: if (d*syn->dt[0]<-dT) return 1; break;
        }
    }
    /* ins loosely coupled position mode */
    if (svr->rtk.opt.mode==PMODE_INS_LGNSS) {
        if (lc==IGCOM_USESOL) { /* for solution data */
            switch (index) {
                case 3: if (d*syn->dt[0]<-dT) return 1; break;
                case 4: if (d*syn->dt[0]> dT) return 1; break;
            }
        }
        if (lc==IGCOM_USEOBS) { /* for observation data */
            switch (index) {
                case 4: if (d*syn->dt[0]> dT) return 1;
                        if (d*syn->dt[1]> dT) return 1; break;
                case 0: if (d*syn->dt[0]<-dT) return 1; break;
                case 1: if (d*syn->dt[1]<-dT) return 1; break;
            }
        }
        if (svr->rtk.opt.insopt.pose_aid||svr->rtk.opt.insopt.align_dualants) {
            switch (index) {
                case 4: if (d*syn->dt[2]<-dT) return 1; break;
                case 6: if (d*syn->dt[2]> dT) return 1; break;
            }
        }
    }
    /* ins tightly coupled position mode */
    if (svr->rtk.opt.mode==PMODE_INS_TGNSS) {
        switch (index) {
            case 4: if (d*syn->dt[0]> dT) return 1;
                    if (d*syn->dt[1]> dT) return 1; break;
            case 0: if (d*syn->dt[0]<-dT) return 1; break;
            case 1: if (d*syn->dt[1]<-dT) return 1; break;
        }
        if (svr->rtk.opt.insopt.pose_aid||svr->rtk.opt.insopt.align_dualants) {
            switch (index) {
                case 4: if (d*syn->dt[2]<-dT) return 1; break;
                case 6: if (d*syn->dt[2]> dT) return 1; break;
            }
        }
    }
    /* visual odometry and ins loosely coupled */
    if (svr->rtk.opt.mode==PMODE_INS_LVO) {
        switch (index) {
            case 4: if (d*syn->dt[0]> dT) return 1; break;
            case 5: if (d*syn->dt[0]<-dT) return 1; break;
        }
    }
    return 0;
}