| | |
| | | { |
| | | return (ts_a - ts_b); |
| | | } |
| | | } |
| | | void CalculateDists(int64_t poll_tx_ts) |
| | | { |
| | | for(int i=0;i<MAX_NEARBASE_NUM;i++) |
| | | { |
| | | |
| | | if(exsistbase_list[i]==KEEP_TIMES) |
| | | { |
| | | |
| | | //temp_freq_offset=freq_offset_filter;//测试 |
| | | memcpy(&resp_tx_timestamp,&anc_resptx[i],4); |
| | | memcpy(&poll_rx_timestamp,&anc_pollrx[i],4); |
| | | treply = ranging_timestamp_diff(resp_tx_timestamp, poll_rx_timestamp); |
| | | //基站 |
| | | // // correct antenna delay |
| | | // treply += ranging_ant_delays_get((uint8_t)(RX_MAIN_ANT_PORT)); |
| | | |
| | | // uint32_t tag_tx_temp=(uint32_t)poll_tx_ts;//测试 |
| | | // memcpy(&tag_polltx[0],&tag_tx_temp,4);//测试 |
| | | |
| | | //poll_tx_timestamp=poll_tx_ts; |
| | | |
| | | memcpy(&poll_tx_timestamp,&tag_polltx[i],4); |
| | | memcpy(&resp_rx_timestamp,&tag_resprx[i],4); |
| | | |
| | | tround=ranging_timestamp_diff(resp_rx_timestamp, poll_tx_timestamp); |
| | | tround_temp=tround; |
| | | treply_temp=treply; |
| | | //标签 |
| | | // correct antenna delay |
| | | // tround -= ranging_ant_delays_get(uwb_app_config.ppdu_params.rx_ant_id); |
| | | // corrected by frequency offset |
| | | tround = (int64_t)((double)tround * (1 - (double)freq_offset_filter / ch_center_freq_map[UWB_CH_NUM])); |
| | | tround_temp=tround; |
| | | treply_temp=treply; |
| | | tof_i = (int32_t)(tround - treply) / 2; |
| | | // tof_i_ui=(uint32_t)tof_i; |
| | | |
| | | // outlier filter去掉负值过滤未添加offset |
| | | // if (tof_i < 0) |
| | | // { |
| | | // tof_i = 0; |
| | | // } |
| | | //tof_f = (double)TIMESTAMP_UNIT_TO_NS((uint32_t)tof_i);//yuan |
| | | tof_f = (double)TIMESTAMP_UNIT_TO_NS(tof_i); |
| | | //distance_tag=(uint16_t)(tof_f * 0.299702547 * VP_VAL - RANGING_CORR);//yuan |
| | | distance_tag=tof_f * 0.299702547 * VP_VAL - RANGING_CORR; |
| | | distance_tag=distance_tag+30;//测试加的offset |
| | | //LOG_INFO(TRACE_MODULE_APP, "distance_tag is %lf\r\n",distance_tag); |
| | | |
| | | if(distance_tag>-1000&&distance_tag<100000) |
| | | { |
| | | nearbase_distlist[i] = distance_tag+(int16_t)g_com_map[OFFSET_AS_TAG];//offset加入上位机校准 |
| | | } |
| | | }else{ |
| | | nearbase_distlist[i] = 0x1ffff; |
| | | } |
| | | |
| | | } |
| | | } |
| | | //#endif |
| | | } |