| | |
|---|
| | | #else |
|---|
| | | clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ; |
|---|
| | | #endif |
|---|
| | | rtd_init = tag_resprx[i] - poll_tx_ts; |
|---|
| | | rtd_init = tag_resprx[i] - poll_tx_ts&0xffffffff; |
|---|
| | | rtd_resp = anc_resptx[i] - anc_pollrx[i]; |
|---|
| | | tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS; |
|---|
| | | distance = tof * SPEED_OF_LIGHT; |
|---|
| | |
|---|
| | | }; |
|---|
| | | |
|---|
| | | } |
|---|
| | | u16 numceshi; |
|---|
| | | float range_lost_time = 0; |
|---|
| | | u16 rec_nearbaseid,rec_nearbasepos; |
|---|
| | | void NearPoll(void) |
|---|
| | |
|---|
| | | uint32_t temp1,temp2,dw_systime; |
|---|
| | | uint32_t frame_len; |
|---|
| | | uint32_t final_tx_time; |
|---|
| | | u32 start_poll; |
|---|
| | | u8 i,j,getsync_flag=0,timeout,flag_ancreadpara=0; |
|---|
| | | u32 start_poll,timeout; |
|---|
| | | u8 i,j,getsync_flag=0,flag_ancreadpara=0; |
|---|
| | | dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置发送后开启接收，并设定延迟时间 |
|---|
| | | dwt_setrxtimeout(0); |
|---|
| | | tag_succ_times = 0; |
|---|
| | |
|---|
| | | dwt_writetxdata(13+4*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去 |
|---|
| | | dwt_writetxfctrl(13+4*nearbase_num, 0);//设置超宽带发送数据长度 |
|---|
| | | dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置 |
|---|
| | | |
|---|
| | | numceshi++; |
|---|
| | | tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0; |
|---|
| | | para_update = 0; |
|---|
| | | get_newbase = 0; |
|---|
| | |
|---|
| | | flag_getresponse=0; |
|---|
| | | start_count=HAL_LPTIM_ReadCounter(&hlptim1); |
|---|
| | | recbase_num=0; |
|---|
| | | timeout=nearbase_num*SLOT_SCALE+50; |
|---|
| | | timeout=nearbase_num*SLOT_SCALE+20; //单位0.1ms |
|---|
| | | end_count=start_count+(timeout<<2); |
|---|
| | | if(end_count>=32768) |
|---|
| | | {end_count-=32768;} |
|---|
| | |
|---|
| | | frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获得接收到的数据长度 |
|---|
| | | dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据 |
|---|
| | | test2 = dwt_readcarrierintegrator(); |
|---|
| | | poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1 |
|---|
| | | resp_rx_ts = get_rx_timestamp_u64(); |
|---|
| | | dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//设定接收超时时间，0位没有超时时间 |
|---|
| | | dwt_rxenable(0);//打开接收 |
|---|
| | | if (rx_buffer[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX]==group_id) //判断接收到的数据是否是response数据 |
|---|
| | | { |
|---|
| | | poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1 |
|---|
| | | resp_rx_ts = get_rx_timestamp_u64(); //获得RESPONSE接收时间T4 |
|---|
| | | // poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1 |
|---|
| | | // resp_rx_ts = get_rx_timestamp_u64(); //获得RESPONSE接收时间T4 |
|---|
| | | recbase_num++; |
|---|
| | | memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2); |
|---|
| | | rec_remotepara_state = rx_buffer[MOTORSTATE_INDEX]>>4; |
|---|
