| | |
|---|
| | | static float clockOffsetRatio; |
|---|
| | | static double rtd_init, rtd_resp; |
|---|
| | | double tof,distance_tag; |
|---|
| | | |
|---|
| | | #define SPEED_OF_LIGHT 299702547 |
|---|
| | | #define DWT_TIME_UNITS (1.0/499.2e6/128.0) //!< = 15.65e-12 s |
|---|
| | | static uint32_t anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM],tag_polltx[MAX_NEARBASE_NUM]; |
|---|
| | |
|---|
| | | int32_t tof_i; |
|---|
| | | uint32_t tof_i_ui; |
|---|
| | | double tof_f; |
|---|
| | | struct mk_uwb_configure |
|---|
| | | { |
|---|
| | | uint8_t phy_work_mode; /* PHY_TX / PHY_RX / PHT_TX|PHY_RX */ |
|---|
| | | struct UWB_CONFIG_T phy_cfg; |
|---|
| | | }; |
|---|
| | | extern struct mk_uwb_configure config; |
|---|
| | | extern int32_t freq_offset,freq_offset_filter; |
|---|
| | | void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset,int32_t anc_clockoffset_from_MK,uint32_t polltx) //写入时间戳信息 |
|---|
| | | { |
|---|
| | |
|---|
| | | // } |
|---|
| | | //} |
|---|
| | | // ts_a - ts_b |
|---|
| | | int64_t ranging_timestamp_diff(int64_t ts_a, int64_t ts_b) |
|---|
| | | int64_t ranging_timestamp_diff1(int64_t ts_a, int64_t ts_b) |
|---|
| | | { |
|---|
| | | if (ts_a < ts_b) |
|---|
| | | { |
|---|
| | |
|---|
| | | } |
|---|
| | | void CalculateDists(int64_t poll_tx_ts) |
|---|
| | | { |
|---|
| | | for(int i=0;i<MAX_NEARBASE_NUM;i++) |
|---|
| | | for(int i=0;i<MAX_NEARBASE_NUM+10;i++) |
|---|
| | | { |
|---|
| | | |
|---|
| | | if(exsistbase_list[i]==KEEP_TIMES) |
|---|
| | | if(taglist_keeptime[i+taglist_current_index]==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); |
|---|
| | | treply = ranging_timestamp_diff1(resp_tx_timestamp, poll_rx_timestamp); |
|---|
| | | //基站 |
|---|
| | | // // correct antenna delay |
|---|
| | | // treply += ranging_ant_delays_get((uint8_t)(RX_MAIN_ANT_PORT)); |
|---|
| | |
|---|
| | | 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=ranging_timestamp_diff1(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 = (int64_t)((double)tround * (1 - (double)anc_clockoffset[i] / uwb_ch_freq_table[config.phy_cfg.ch_num]));//此处有修改 |
|---|
| | | tround_temp=tround; |
|---|
| | | treply_temp=treply; |
|---|
| | | tof_i = (int32_t)(tround - treply) / 2; |
|---|
| | |
|---|
| | | |
|---|
| | | if(distance_tag>-1000&&distance_tag<100000) |
|---|
| | | { |
|---|
| | | nearbase_distlist[i] = distance_tag+(int16_t)g_com_map[OFFSET_AS_TAG];//offset加入上位机校准 |
|---|
| | | } |
|---|
| | | taglist_dist[i+taglist_current_index] = distance_tag+(int16_t)g_com_map[DIST_OFFSET];//offset未加 |
|---|
| | | }else{ |
|---|
| | | taglist_dist[i] = 0x2ffff; |
|---|
| | | } |
|---|
| | | }else{ |
|---|
| | | nearbase_distlist[i] = 0x1ffff; |
|---|
| | | //nearbase_distlist[i] = 0x7fff; |
|---|
| | | } |
|---|
| | | |
|---|
| | | } |
|---|
