| | |
| | | } |
| | | tyncpoll_time=(tagslotpos-1)*slottime; |
| | | } |
| | | mainbase_id=0;//minid; |
| | | mainbase_id=minid; |
| | | } |
| | | |
| | | u8 FindNearBasePos(u16 baseid) |
| | |
| | | return i; |
| | | } |
| | | } |
| | | u8 recbase_num=0; |
| | | #define CHANGE_BASE_THRESHOLD 5 |
| | | void NearAncSelect(void) |
| | | {static u16 last_mainbase_id,change_base_count; |
| | | int32_t nearbase_mindist=99999, nearbase_minpos; |
| | | u8 i; |
| | | for(i=0;i<nearbase_num;i++) |
| | | for(i=0;i<recbase_num-1;i++) |
| | | { |
| | | if(nearbase_mindist<nearbase_distlist[i]) |
| | | if(nearbase_mindist>nearbase_distlist[i]) |
| | | { |
| | | nearbase_mindist=nearbase_distlist[i]; |
| | | nearbase_minpos=i; |
| | |
| | | uint32_t frame_len; |
| | | uint32_t final_tx_time; |
| | | u32 start_poll; |
| | | u8 i,getsync_flag=0,recbase_num,timeout; |
| | | u8 i,getsync_flag=0,timeout; |
| | | dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //设置åéåå¼å¯æ¥æ¶ï¼å¹¶è®¾å®å»¶è¿æ¶é´ |
| | | dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); |
| | | tag_succ_times = 0; |
| | |
| | | memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2); |
| | | tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_POLL; |
| | | memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2); |
| | | dwt_writetxdata(24, tx_near_msg, 0);//å°Pollå
æ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå» |
| | | dwt_writetxfctrl(24, 0);//设置è¶
宽带åéæ°æ®é¿åº¦ |
| | | dwt_writetxdata(13+2*nearbase_num, tx_near_msg, 0);//å°Pollå
æ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå» |
| | | dwt_writetxfctrl(13+2*nearbase_num, 0);//设置è¶
宽带åéæ°æ®é¿åº¦ |
| | | dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//å¼å¯åéï¼åé宿åçå¾
䏿®µæ¶é´å¼å¯æ¥æ¶ï¼çå¾
æ¶é´å¨dwt_setrxaftertxdelayä¸è®¾ç½® |
| | | |
| | | flag_finalsend=0; |
| | | neartimout_timer=0; |
| | | recbase_num=0; |
| | | timeout=nearbase_num/4+12; |
| | | timeout=nearbase_num/4+2; |
| | | mainbase_dist=100000; |
| | | mainbase_lost_count++; |
| | | while(neartimout_timer<timeout) |
| | |
| | | if(dw_systime>temp1&&dw_systime<temp2) |
| | | { |
| | | flag_finalsend=0; |
| | | dwt_forcetrxoff(); |
| | | dwt_setdelayedtrxtime(final_tx_time); |
| | | result=dwt_starttx(DWT_START_TX_DELAYED);//设å®ä¸ºå»¶è¿åé |
| | | break; |
| | | } |
| | | } |
| | | if(neartimout_timer>timeout) |
| | |
| | | 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); //è¯»åæ¥æ¶æ°æ® |
| | | //dwt_setrxtimeout(DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//è®¾å®æ¥æ¶è¶
æ¶æ¶é´ï¼0使²¡æè¶
æ¶æ¶é´ |
| | | dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//è®¾å®æ¥æ¶è¶
æ¶æ¶é´ï¼0使²¡æè¶
æ¶æ¶é´ |
| | | dwt_rxenable(0);//æå¼æ¥æ¶ |
| | | if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ® |
| | | { u16 rec_nearbaseid,rec_nearbasepos; |
| | |
| | | flag_finalsend=1; |
| | | memcpy(&mainbase_dist,&rx_buffer[DIST_IDX],4); |
| | | final_tx_time = (resp_rx_ts + ((RESP_RX_TO_FINAL_TX_DLY_UUS+DELAY_BETWEEN_TWO_FRAME_UUS*nearbase_num+500) * UUS_TO_DWT_TIME)) >> 8; |
| | | temp1=final_tx_time-((200*UUS_TO_DWT_TIME)>>8); |
| | | temp1=final_tx_time-((350*UUS_TO_DWT_TIME)>>8); |
| | | temp2=final_tx_time+((100*UUS_TO_DWT_TIME)>>8); |
| | | dwt_setdelayedtrxtime(final_tx_time);//设置finalå
åéæ¶é´T5 |
| | | // 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_near_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//å°T1ï¼T4ï¼T5åå
¥åéæ°æ® |
| | | final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX], resp_rx_ts); |
| | |
| | | tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL; |
| | | dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//å°åéæ°æ®åå
¥DW1000 |
| | | dwt_writetxfctrl(28+nearbase_num*4, 0);//设å®åéæ°æ®é¿åº¦ |
| | | usart_send[2] = 1;//æ£å¸¸æ¨¡å¼ |
| | | usart_send[3] = 17;//æ°æ®æ®µé¿åº¦ |
| | | usart_send[4] = frame_seq_nb;//æ°æ®æ®µé¿åº¦ |
| | | memcpy(&usart_send[5],&dev_id,2); |
| | | memcpy(&usart_send[7],&rec_nearbaseid,2); |
| | | memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4); |
| | | usart_send[13] = battary; |
| | | usart_send[14] = button; |
| | | checksum = Checksum_u16(&usart_send[2],17); |
| | | memcpy(&usart_send[19],&checksum,2); |
| | | UART_PushFrame(usart_send,21); |
| | | //result=dwt_starttx(DWT_START_TX_DELAYED);//设å®ä¸ºå»¶è¿åé |
| | | }else{ |
| | | rec_nearbasepos=FindNearBasePos(rec_nearbaseid); |
| | |
| | | dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//å°åéæ°æ®åå
¥DW1000 |
| | | dwt_writetxfctrl(28+nearbase_num*4, 0);//设å®åéæ°æ®é¿åº¦ |
| | | LED0_BLINK; |
| | | if(rec_nearbasepos+1==nearbase_num) |
| | | { |
| | | result=dwt_starttx(DWT_START_TX_DELAYED);//设å®ä¸ºå»¶è¿åé |
| | | break; |
| | | } |
| | | usart_send[2] = 1;//æ£å¸¸æ¨¡å¼ |
| | | usart_send[3] = 17;//æ°æ®æ®µé¿åº¦ |
| | | usart_send[4] = frame_seq_nb;//æ°æ®æ®µé¿åº¦ |
| | | memcpy(&usart_send[5],&dev_id,2); |
| | | memcpy(&usart_send[7],&rec_nearbaseid,2); |
| | | memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4); |
| | | usart_send[13] = battary; |
| | | usart_send[14] = button; |
| | | checksum = Checksum_u16(&usart_send[2],17); |
| | | memcpy(&usart_send[19],&checksum,2); |
| | | UART_PushFrame(usart_send,21); |
| | | |
| | | //dwt_setdelayedtrxtime(final_tx_time);//设置finalå
åéæ¶é´T5 |
| | | // result=dwt_starttx(DWT_START_TX_DELAYED);//设å®ä¸ºå»¶è¿åé |
| | | //dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4);//å°åéæ°æ®åå
¥DW1000 |
| | |
| | | |
| | | memcpy(&tx_near_msg[DIST_IDX], &tagdist_list[taglist_pos], 4); |
| | | tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE; |
| | | dwt_writetxdata(17, tx_near_msg, 0);//åå
¥åéæ°æ® |
| | | dwt_writetxfctrl(17, 0);//设å®åéé¿åº¦ |
| | | dwt_writetxdata(20, tx_near_msg, 0);//åå
¥åéæ°æ® |
| | | dwt_writetxfctrl(20, 0);//设å®åéé¿åº¦ |
| | | result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//å»¶è¿åéï¼çå¾
æ¥æ¶ |
| | | |
| | | battary = rx_buffer[BATTARY_IDX]; |
| | |
| | | frame_seq_nb = rx_buffer[SEQUENCE_IDX]; |
| | | if(result==0) |
| | | { |
| | | while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///䏿æ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæè
åºç°é误 |
| | | while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///䏿æ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæè
åºç°é误 |
| | | { }; |
| | | }else{ |
| | | result++; |
| | | } |
| | | if (status_reg & SYS_STATUS_RXFCG)//æ¥æ¶æå |
| | | { |
| | |
| | | { |
| | | u8 send_len,i; |
| | | u16 tempid; |
| | | g_start_sync_flag=0; |
| | | dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶
æ¶æ¶é´ï¼0使²¡æè¶
æ¶æ¶é´ |
| | | dwt_rxenable(0);//æå¼æ¥æ¶ |
| | | while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//䏿æ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæè
åºç°é误 |