| | |
|---|
| | | #define POLL_TX_TO_RESP_RX_DLY_UUS 150 |
|---|
| | | /* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the |
|---|
| | | * frame length of approximately 2.66 ms with above configuration. */ |
|---|
| | | #define RESP_RX_TO_FINAL_TX_DLY_UUS 410 |
|---|
| | | #define RESP_RX_TO_FINAL_TX_DLY_UUS 510 |
|---|
| | | |
|---|
| | | /* Receive response timeout. See NOTE 5 below. */ |
|---|
| | | #define RESP_RX_TIMEOUT_UUS 600 |
|---|
| | |
|---|
| | | DISCPOLL, |
|---|
| | | GETNEARMSG, |
|---|
| | | NEARPOLL, |
|---|
| | | }tag_state=GETNEARMSG; |
|---|
| | | }tag_state=DISCPOLL; |
|---|
| | | static dwt_config_t config = { |
|---|
| | | 2, /* Channel number. */ |
|---|
| | | DWT_PRF_64M, /* Pulse repetition frequency. */ |
|---|
| | |
|---|
| | | u32 rec_tagpos_binary; |
|---|
| | | int16_t offset=2700; |
|---|
| | | u8 motor_state,rec_remotepara_state,rec_remotepara[80]; |
|---|
| | | void Poll(void) |
|---|
| | | { |
|---|
| | | uint32_t frame_len; |
|---|
| | | uint32_t final_tx_time; |
|---|
| | | u32 start_poll; |
|---|
| | | int32_t mindist=999999,minid=-1,temp_dist; |
|---|
| | | u8 i,getsync_flag=0; |
|---|
| | | dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置发送后开启接收，并设定延迟时间 |
|---|
| | | dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); |
|---|
| | | tag_succ_times = 0; |
|---|
| | | |
|---|
| | | for(i=0;i<3;i++) |
|---|
| | | { |
|---|
| | | /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */ |
|---|
| | | tx_poll_msg[ANC_TYPE_IDX] = i; |
|---|
| | | dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去 |
|---|
| | | dwt_writetxfctrl(sizeof(tx_poll_msg), 0);//设置超宽带发送数据长度 |
|---|
| | | dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置 |
|---|
| | | |
|---|
| | | /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */ |
|---|
| | | while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误 |
|---|
| | | { |
|---|
| | | }; |
|---|
| | | if(status_reg==0xffffffff) |
|---|
| | | { |
|---|
| | | NVIC_SystemReset(); |
|---|
| | | } |
|---|
| | | if (status_reg & SYS_STATUS_RXFCG)//如果成功接收 |
|---|
| | | { |
|---|
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位 |
|---|
| | | frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获得接收到的数据长度 |
|---|
| | | dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据 |
|---|
| | | if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据 |
|---|
| | | { |
|---|
| | | poll_tx_ts = get_tx_timestamp_u64(); //获得POLL发送时间T1 |
|---|
| | | resp_rx_ts = get_rx_timestamp_u64(); //获得RESPONSE接收时间T4 |
|---|
| | | if(getsync_flag==0) |
|---|
| | | { |
|---|
| | | getsync_flag=1; |
|---|
| | | memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2); |
|---|
| | | memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2); |
|---|
| | | current_count=HAL_LPTIM_ReadCounter(&hlptim1); |
|---|
| | | // memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2); |
|---|
| | | tmp_time=tmp_time+450; |
|---|
| | | if(tmp_time>999) |
|---|
| | | { |
|---|
| | | tmp_time-=999; |
|---|
| | | sync_timer++; |
|---|
| | | if(sync_timer>=1010) |
|---|
| | | {sync_timer=0;} |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | memcpy(&temp_dist, &rx_buffer[DIST_IDX], 4); |
|---|
| | | memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4); |
|---|
| | | /* Compute final message transmission time. See NOTE 9 below. */ |
|---|
| | | final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间，T5=T4+Treply2 |
|---|
| | | dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5 |
|---|
| | | final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay |
|---|
| | | final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据 |
|---|
| | | final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts); |
|---|
| | | final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts); |
|---|
| | | dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//将发送数据写入DW1000 |
|---|
| | | dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度 |
|---|
| | | result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送 |
|---|
| | | tag_succ_times++; |
|---|
| | | LED0_BLINK; |
|---|
| | | |
|---|
| | | tagslotpos=GetRandomSlotPos(rec_tagpos_binary); |
|---|
| | | tyncpoll_time = (tagslotpos--%max_slotpos)*slottime; |
|---|
| | | SetNextPollTime(tyncpoll_time); |
|---|
| | | |
|---|
| | | if(temp_dist<mindist&&temp_dist!=0) |
|---|
| | | { |
|---|
| | | memcpy(&minid, &rx_buffer[ANCHOR_ID_IDX], 4); |
|---|
| | | tag_state=GETNEARMSG; |
|---|
| | | trygetnearmsg_times=5; |
|---|
| | | mindist=temp_dist; |
|---|
| | | } |
|---|
| | | if(result==0) |
|---|
| | | {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成 |
|---|
| | | { }; |
|---|
| | | } |
|---|
| | | /* Clear TXFRS event. */ |
|---|
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除标志位 |
|---|
| | | random_delay_tim = 0; |
|---|
| | | } |
|---|
| | | else |
|---|
| | | { |
|---|
| | | random_delay_tim = DFT_RAND_DLY_TIM_MS; //如果通讯失败，将间隔时间增加5ms，避开因为多标签同时发送引起的冲突。 |
|---|
| | | } |
|---|
| | | } |
|---|
| | | else |
|---|
| | | { |
|---|
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); |
|---|
| | | random_delay_tim = DFT_RAND_DLY_TIM_MS; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | if(getsync_flag==0) |
|---|
| | | { |
|---|
| | | tagslotpos--; |
|---|
| | | if(tagslotpos==0||tagslotpos>max_slotpos) |
|---|
| | | { |
|---|
| | | tagslotpos=max_slotpos; |
|---|
| | | } |
|---|
| | | tyncpoll_time=(tagslotpos-1)*slottime; |
|---|
| | | } |
|---|
| | | mainbase_id=minid; |
|---|
| | | } |
|---|
| | | u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;; |
|---|
| | | void GetNearMsg(void) |
|---|
| | | { |
|---|
| | | u32 start_poll,frame_len; |
|---|
| | | |
|---|
| | | |
|---|
| | | memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2); |
|---|
| | | memcpy(&tx_near_msg[TAG_ID_IDX],&dev_id,2); |
|---|
| | | tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG; |
|---|
| | | |
|---|
| | | dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置发送后开启接收，并设定延迟时间 |
|---|
| | | dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); |
|---|
| | | dwt_writetxdata(12, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去 |
|---|
| | | dwt_writetxfctrl(12, 0);//设置超宽带发送数据长度 |
|---|
| | | dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED); |
|---|
| | | |
|---|
| | | /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */ |
|---|
| | | while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误 |
|---|
| | | { |
|---|
| | | }; |
|---|
| | | if(status_reg==0xffffffff) |
|---|
| | | { |
|---|
| | | NVIC_SystemReset(); |
|---|
| | | } |
|---|
| | | if (status_reg & SYS_STATUS_RXFCG)//如果成功接收 |
|---|
| | | { |
|---|
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位 |
|---|
| | | frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获得接收到的数据长度 |
|---|
| | | dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据 |
|---|
| | | if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据 |
|---|
| | | { |
|---|
| | | nearbase_num=rx_buffer[NEARBASENUM_INDEX]; |
|---|
| | | memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4); |
|---|
| | | tagslotpos=GetRandomSlotPos(rec_tagpos_binary); |
|---|
| | | //tagslotpos=rx_buffer[TAGSLOTPOS]; |
|---|
| | | memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2); |
|---|
| | | //slottime=ceil((nearbase_num+2)*0.3)+1; |
|---|
| | | tyncpoll_time=tagslotpos*slottime; |
|---|
| | | tag_state=NEARPOLL; |
|---|
| | | } |
|---|
| | | }else{ |
|---|
| | | tyncpoll_time = (tagslotpos--%max_slotpos)*slottime; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | void NearPoll(void) |
|---|
| | | { |
|---|
| | | |
|---|
| | |
|---|
| | | HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET); |
|---|
| | | |
|---|
| | | } |
|---|
| | | u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32]; |
|---|
| | | void GetNearMsg(void) |
|---|
| | | { |
|---|
| | | u32 start_poll,frame_len; |
|---|
| | | if(nearmsg_mainbase) |
|---|
| | | {nearmsg_mainbase=0; |
|---|
| | | }else{ |
|---|
| | | nearmsg_mainbase=1; |
|---|
| | | } |
|---|
| | | memcpy(&tx_near_msg[ANCHOR_ID_IDX],&nearmsg_mainbase,2); |
|---|
| | | memcpy(&tx_near_msg[TAG_ID_IDX],&dev_id,2); |
|---|
| | | tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG; |
|---|
| | | |
|---|
| | | dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置发送后开启接收，并设定延迟时间 |
|---|
| | | dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); |
|---|
| | | dwt_writetxdata(12, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去 |
|---|
| | | dwt_writetxfctrl(12, 0);//设置超宽带发送数据长度 |
|---|
| | | dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED); |
|---|
| | | |
|---|
| | | /* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */ |
|---|
| | | while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误 |
|---|
| | | { |
|---|
| | | }; |
|---|
| | | if(status_reg==0xffffffff) |
|---|
| | | { |
|---|
| | | NVIC_SystemReset(); |
|---|
| | | } |
|---|
| | | if (status_reg & SYS_STATUS_RXFCG)//如果成功接收 |
|---|
| | | { |
|---|
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位 |
|---|
| | | frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //获得接收到的数据长度 |
|---|
| | | dwt_readrxdata(rx_buffer, frame_len, 0); //读取接收数据 |
|---|
| | | if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据 |
|---|
| | | { |
|---|
| | | nearbase_num=rx_buffer[NEARBASENUM_INDEX]; |
|---|
| | | memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4); |
|---|
| | | tagslotpos=GetRandomSlotPos(rec_tagpos_binary); |
|---|
| | | //tagslotpos=rx_buffer[TAGSLOTPOS]; |
|---|
| | | memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2); |
|---|
| | | //slottime=ceil((nearbase_num+2)*0.3)+1; |
|---|
| | | tyncpoll_time=tagslotpos*slottime; |
|---|
| | | tag_state=NEARPOLL; |
|---|
| | | } |
|---|
| | | }else{ |
|---|
| | | tyncpoll_time = (tagslotpos--%max_slotpos)*slottime; |
|---|
| | | } |
|---|
| | | } |
|---|
| | | |
|---|
| | | void Tag_App(void)//发送模式(TAG标签) |
|---|
| | | { |
|---|
| | |
|---|
| | | switch(tag_state) |
|---|
| | | { |
|---|
| | | case DISCPOLL: |
|---|
| | | // Poll(); |
|---|
| | | Poll(); |
|---|
| | | break; |
|---|
| | | case GETNEARMSG: |
|---|
| | | |
|---|
