Multi-Sensor sync with trigger signal(period 1S)

       //sync
    int reg_tranfer_down = 63;
    int ii,valid_pack,pack_num;
    int rem_head = 0;
    
    double *rem_interp;
    int n=48*(1+reg_tranfer_down);
    rem_interp = malloc(sizeof(double)*(1<<24));
    unsigned int trig_num=0;
    unsigned int tick_prev=0;
    long long interp_head=0;
    double rt = 1;
    long long interp_tail=0;
    MeasureResult_t *pv6buff;
	int init_flag = 1;
	int pred_tick;
	int ttime;
	int d1;
	int tick_cur,tick2_prev,tick2_cur;
	double tick_org,tick_actual;
	double *rem_prev=malloc(256*8);;
	double *rem_cur;
    int tick2_start;
    for(int k=0;k<(packnum/1);k++){
        pack_num = 1;
        pv6buff = &res_array[k*1];

        for ( ii=0,valid_pack=0;ii<pack_num;ii++) {
			rem_cur = (double *)pv6buff[ii].rem_angle;
			tick_cur = (int)(pv6buff[ii].tick & 0x7fffffff);
            if (tick_cur < tick_prev){
				//calc t0,t1,rt
				pred_tick = tick_prev+256*n -240e6;
				if(pred_tick<0)
					pred_tick = pred_tick+256*n;
				ttime = tick_cur - pred_tick;
				rt = 1+(double)(ttime)/240e6;
				trig_num++;
                printf("%d:\trt=%.7llf, tdiff=%d, tick=%d,\tpred_tick=%d\n",trig_num,rt,ttime,tick_cur,pred_tick);
				
				if(init_flag){
					d1 = 256 - ceil((double)tick_cur/n);
					for(int tick2_cur=0;tick2_cur<(tick_cur-n);tick2_cur+=n){
						tick_org=tick_cur - n*(257-d1);
						tick_actual=tick_org*rt;
						rem_interp[interp_head&((1<<24)-1)] = (rem_prev[d1]+(rem_prev[d1+1]-rem_prev[d1])*(tick2_cur-tick_actual)/(n*rt));
						interp_head++;
						tick2_prev = tick2_cur;
						d1++;
					}
					init_flag = 0;
				}
            }

			if(init_flag==0){
				tick2_start = tick2_prev + n;
				if(tick2_start>240e6)
					tick2_start = tick2_start - 240e6;
				d1 = 0;
				for(int tick2_cur=tick2_start;tick2_cur<(tick_cur+255*n)*rt;tick2_cur+=n){
					tick_org = tick_cur + (d1-1)*n;
					tick_actual = tick_org*rt;
					if(tick2_cur-tick_actual<0){
						d1=d1-1;
						tick_org = tick_cur + (d1 - 1)*n;
						tick_actual = tick_org*rt;
					}
					if(tick2_cur-tick_actual>n*rt){
						d1=d1+1;
						tick_org = tick_cur + (d1 - 1)*n;
						tick_actual = tick_org*rt;
					}
					if(d1==0){
						rem_interp[interp_head&((1<<24)-1)] = rem_prev[255]+(rem_cur[0]-rem_prev[255])*(tick2_cur-tick_actual)/(n*rt);
						interp_head++;
					}else{
						rem_interp[interp_head&((1<<24)-1)] = rem_cur[d1-1]+(rem_cur[d1+1-1]-rem_cur[d1-1])*(tick2_cur-tick_actual)/(n*rt);
						interp_head++;
					}
					d1=d1+1;
					tick2_prev = tick2_cur;
				}
			}
			tick_prev = tick_cur;
			memcpy(rem_prev,pv6buff[ii].rem_angle,8*256);
        }
        

    }

评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值