| | |
| | | uint32_t g_UWB_com_interval = 0; |
| | | float dis_after_filter; //å½åè·ç¦»å¼ |
| | | LPFilter_Frac* p_Dis_Filter; //æµè·ç¨çä½éæ»¤æ³¢å¨ |
| | | uint16_t g_Tagdist[TAG_NUM_IN_SYS]; |
| | | int32_t g_Tagdist[TAG_NUM_IN_SYS]; |
| | | uint8_t g_flag_Taggetdist[256]; |
| | | |
| | | static uint64_t get_tx_timestamp_u64(void) |
| | |
| | | *ts += ts_field[i] << (i * 8); |
| | | } |
| | | } |
| | | extern int32_t tagdist_list[TAG_NUM_IN_SYS]; |
| | | void TagDistClear(void) |
| | | { |
| | | static uint16_t clear_judge_cnt; |
| | |
| | | g_flag_Taggetdist[i]++; |
| | | if(g_flag_Taggetdist[i]>=2) |
| | | { |
| | | g_Tagdist[i]=0xffff; |
| | | tagdist_list[i]=0x1ffff; |
| | | } |
| | | } |
| | | } |
| | |
| | | } |
| | | return i; |
| | | } |
| | | u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0; |
| | | uint32_t frame_len; |
| | | uint32_t resp_tx_time; |
| | | uint8_t rec_nearbase_num,anc_report_num; |
| | |
| | | LED0_BLINK; //æ¯æå䏿¬¡é讯åéªç䏿¬¡ |
| | | dis_after_filter=dist_cm; |
| | | hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]; |
| | | if(abs(hex_dist-his_dist[taglist_pos])<1000) |
| | | { |
| | | tagdist_list[taglist_pos] = hex_dist; |
| | | } |
| | | his_dist[taglist_pos]=hex_dist; |
| | | if(hex_dist>-1000&&hex_dist<200000) |
| | | { |
| | | g_flag_Taggetdist[taglist_pos]=0; |
| | | if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3) |
| | | { |
| | | misdist_num[taglist_pos]=0; |
| | | tagdist_list[taglist_pos] = hex_dist; |
| | | his_dist[taglist_pos]=hex_dist; |
| | | g_Tagdist[taglist_pos]=hex_dist; |
| | | #ifndef USART_INTEGRATE_OUTPUT |
| | | usart_send[2] = 1;//æ£å¸¸æ¨¡å¼ |
| | | usart_send[3] = 17;//æ°æ®æ®µé¿åº¦ |
| | | usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦ |
| | | memcpy(&usart_send[5],&tag_id_recv,2); |
| | | memcpy(&usart_send[7],&dev_id,2); |
| | | memcpy(&usart_send[9],&tagdist_list[taglist_pos],4); |
| | | usart_send[13] = battary; |
| | | usart_send[14] = button; |
| | | usart_send[15] = firstpath_power; |
| | | usart_send[16] = (rx_power-firstpath_power)*10; |
| | | usart_send[17] = tag_frequency; |
| | | usart_send[18] = tag_slotpos; |
| | | checksum = Checksum_u16(&usart_send[2],17); |
| | | memcpy(&usart_send[19],&checksum,2); |
| | | // UART_PushFrame(usart_send,21); |
| | | #else |
| | | memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2); |
| | | memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4); |
| | | anc_report_num++; |
| | | #endif |
| | | #ifdef DEBUG_INF |
| | | printf("åºå·ï¼%d\r\n",frame_seq_nb2); |
| | | #endif |
| | | }else{ |
| | | // printf("%d",hex_dist); |
| | | misdist_num[taglist_pos]++; |
| | | } |
| | | } |
| | | } |
| | | }else{ |
| | | dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); |
| | |
| | | extern uint16_t configremotetagID; |
| | | extern u8 remotetag_paralen; |
| | | u8 finalsend=0; |
| | | u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0; |
| | | |
| | | u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base |
| | | { |
| | | |
| | |
| | | dwt_setdelayedtrxtime(resp_tx_time);//设置Responseåéæ¶é´T3 |
| | | dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//设置åé宿åå¼å¯æ¥æ¶å»¶è¿æ¶é´ |
| | | dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//æ¥æ¶è¶
æ¶æ¶é´ |
| | | dwt_readdiagnostics(&d1); |
| | | //dwt_readdiagnostics(&d1); |
| | | |
| | | if(new_tagid) |
| | | { |
| | |
| | | LED0_BLINK; //æ¯æå䏿¬¡é讯åéªç䏿¬¡ |
| | | dis_after_filter=dist_cm; |
| | | hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]; |
| | | g_flag_Taggetdist[taglist_pos]=0; |
| | | |
| | | if(hex_dist>-1000&&hex_dist<200000) |
| | | { |
| | | g_flag_Taggetdist[taglist_pos]=0; |
| | | if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3) |
| | | { |
| | | misdist_num[taglist_pos]=0; |
| | |
| | | extern u16 synclost_count; |
| | | uint32_t current_syncid=0xffffffff,synclost_timer; |
| | | extern u8 flag_syncbase,waitsync_flag; |
| | | u8 tagpos_rec[50],tagpos_send[50],ancidlist_num; |
| | | u8 tagpos_rec[50],tagpos_send[50],ancidlist_num,zero_matrix[100]={0}; |
| | | u16 ancidlist_rec[20],ancidlist_send[20]; |
| | | u16 recnearbaselist_id[30]; |
| | | int32_t recnearbaselist_dist[30]; |
| | |
| | | switch(rx_buffer[MESSAGE_TYPE_IDX]) |
| | | { |
| | | uint16_t checksum; |
| | | case POLL: |
| | | if (anchor_type == rx_buffer[ANC_TYPE_IDX]) |
| | | case SPOLL: |
| | | //if (anchor_type == rx_buffer[ANC_TYPE_IDX]) |
| | | if(dev_id==anc_id_recv) |
| | | { |
| | | taglist_pos=CmpTagInList(tag_id_recv); |
| | | if(taglist_pos==taglist_num) |
| | | { |
| | | taglist_pos=taglist_num; |
| | | tagid_list[taglist_num++]=tag_id_recv; |
| | | |
| | | new_tagid=1; |
| | | }else{ |
| | | new_tagid=0; |
| | | } |
| | | tagofflinetime[taglist_pos]=0; |
| | | |
| | | rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX]; |
| | | battary = rx_buffer[BATTARY_IDX]; |
| | | button = rx_buffer[BUTTON_IDX]; |
| | | frame_seq_nb2 = rx_buffer[SEQUENCE_IDX]; |
| | | tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX]; |
| | | tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX]; |
| | | |
| | | ////////////////åºç«æ±æ»æ¨¡å¼æ°æ® |
| | | memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2); |
| | | memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2); |
| | | memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4); |
| | | usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼ |
| | | usart_send[3] = 15+8*(rec_nearbase_num);//æ°æ®æ®µé¿åº¦ |
| | | memcpy(&usart_send[4],&tag_id_recv,2); |
| | | usart_send[6] = rx_buffer[SEQUENCE_IDX]; |
| | | usart_send[7] = rx_buffer[SEQUENCEH_IDX]; |
| | | usart_send[8] = battary; |
| | | usart_send[9] = button; |
| | | memcpy(&usart_send[10],&rec_tagheight,2); |
| | | usart_send[12] = tag_frequency; |
| | | usart_send[13] = tag_slotpos; |
| | | usart_send[14] = 0; |
| | | usart_send[15] = 0; |
| | | usart_send[16] = rec_nearbase_num; |
| | | |
| | | memcpy(&usart_send[17],&recnearbaselist_id,2*rec_nearbase_num); |
| | | memcpy(&usart_send[17+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num); |
| | | memcpy(&usart_send[17+rec_nearbase_num*6],zero_matrix,2*rec_nearbase_num); |
| | | |
| | | checksum = Checksum_u16(&usart_send[2],15+8*rec_nearbase_num); |
| | | memcpy(&usart_send[17+8*rec_nearbase_num],&checksum,2); |
| | | UART_PushFrame(usart_send,19+8*rec_nearbase_num); |
| | | Anchor_RecPoll(); |
| | | } |
| | | break; |
| | | case SYNC: |
| | | memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4); |
| | |
| | | memcpy(&usart_send[6],&tag_id_recv,2); |
| | | memcpy(&usart_send[8],&rx_buffer[10],9); |
| | | usart_send[17] = firstpath_power; |
| | | memcpy(&usart_send[18],&rx_buffer[19],2); |
| | | checksum = Checksum_u16(&usart_send[2],20); |
| | | memcpy(&usart_send[22],&checksum,2); |
| | | UART_PushFrame(usart_send,24); |
| | |
| | | usart_send[7] = rx_buffer[SEQUENCEH_IDX]; |
| | | usart_send[8] = battary; |
| | | usart_send[9] = button; |
| | | |
| | | memcpy(&usart_send[10],&rec_tagheight,2); |
| | | usart_send[12] = tag_frequency; |
| | | usart_send[13] = tag_slotpos; |