//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);
}
}
Multi-Sensor sync with trigger signal(period 1S)
最新推荐文章于 2024-10-03 23:25:47 发布