From 0f9a5a119401d9e08cefc4b9cb7f700988a69abe Mon Sep 17 00:00:00 2001 From: zhyinch <zhyinch@gmail.com> Date: 星期六, 04 六月 2022 17:01:05 +0800 Subject: [PATCH] V1.70修改免报距离不生效bug --- 源码/核心板/Src/application/dw_app.c | 148 ++++++++++++++++++++++++++++++++++--------------- 1 files changed, 102 insertions(+), 46 deletions(-) diff --git "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" index 17a2dd3..dac4566 100644 --- "a/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" +++ "b/\346\272\220\347\240\201/\346\240\270\345\277\203\346\235\277/Src/application/dw_app.c" @@ -48,8 +48,8 @@ uint32_t g_UWB_com_interval = 0; float dis_after_filter; //当前距离值 LPFilter_Frac* p_Dis_Filter; //测距用的低通滤波器 -uint16_t g_Tagdist[TAG_NUM_IN_SYS]; -uint8_t g_flag_Taggetdist[256]; +int32_t g_Tagdist[TAG_NUM_IN_SYS]; +uint8_t g_flag_Taggetdist[256],flag_tag_distsmooth[TAG_NUM_IN_SYS]; static uint64_t get_tx_timestamp_u64(void) { @@ -110,11 +110,12 @@ g_flag_Taggetdist[i]++; if(g_flag_Taggetdist[i]>=2) { - g_Tagdist[i]=0xffff; + g_Tagdist[i]=0x1ffff; } } } } +extern int16_t g_commap_antdelay; void Dw1000_Init(void) { /* Reset and initialise DW1000. @@ -131,7 +132,7 @@ /* Apply default antenna delay value. See NOTE 1 below. */ dwt_setrxantennadelay(RX_ANT_DLY); //设置接收天线延迟 - dwt_settxantennadelay(TX_ANT_DLY); //设置发射天线延迟 + dwt_settxantennadelay(0); //设置发射天线延迟 /* Set expected response's delay and timeout. See NOTE 4 and 5 below. * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */ @@ -343,18 +344,21 @@ tx_nearpoll_msg[BUTTON_IDX] = !READ_KEY0; tx_nearpoll_msg[SEQUENCE_IDX] = frame_seq_nb++; tx_nearpoll_msg[NEARBASENUM_INDEX] = nearbase_num; + memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2); for(i=0;i<nearbase_num;i++) { memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX+nearbase_num*2+i*2],&nearbase_distlist[i],2);} - + + memcpy(&tx_nearpoll_msg[NEARBASEID_INDEX+nearbase_num*4],&g_commap_antdelay,2); + for(i=0;i<MAX_NEARBASE_NUM;i++) { nearbase_distlist[i]=0x1ffff; } tx_nearpoll_msg[MESSAGE_TYPE_IDX] = NEAR_POLL; memcpy(&tx_nearpoll_msg[ANCHOR_ID_IDX],&mainbase_id,2); - dwt_writetxdata(13+4*nearbase_num, tx_nearpoll_msg, 0);//将Poll包数据传给DW1000,将在开启发送时传出去 - dwt_writetxfctrl(13+4*nearbase_num, 0);//设置超宽带发送数据长度 + dwt_writetxdata(15+4*nearbase_num, tx_nearpoll_msg, 0);//将Poll包数据传给DW1000,将在开启发送时传出去 + dwt_writetxfctrl(15+4*nearbase_num, 0);//设置超宽带发送数据长度 dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送,发送完成后等待一段时间开启接收,等待时间在dwt_setrxaftertxdelay中设置 flag_finalsend=0; @@ -434,6 +438,7 @@ } TIM3->CNT=tmp_time; } + if(g_com_map[DEV_ROLE]) current_slotnum=temp_sync_timer1/g_com_map[COM_INTERVAL]; // if(tagslotpos>max_slotpos) // tagslotpos=tagslotpos%(max_slotpos+1); @@ -469,14 +474,10 @@ usart_send[2] = 1;//正常模式 usart_send[3] = 17;//数据段长度 usart_send[4] = frame_seq_nb;//数据段长度 - if(g_com_map[DEV_ROLE]) - { + memcpy(&usart_send[5],&dev_id,2); memcpy(&usart_send[7],&rec_nearbaseid,2); - }else{ - memcpy(&usart_send[5],&rec_nearbaseid,2); - memcpy(&usart_send[7],&dev_id,2); - } + memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4); usart_send[13] = battary; usart_send[14] = button; @@ -519,14 +520,10 @@ usart_send[2] = 1;//正常模式 usart_send[3] = 17;//数据段长度 usart_send[4] = frame_seq_nb;//数据段长度 - if(g_com_map[DEV_ROLE]) - { - memcpy(&usart_send[5],&dev_id,2); - memcpy(&usart_send[7],&rec_nearbaseid,2); - }else{ + memcpy(&usart_send[5],&rec_nearbaseid,2); memcpy(&usart_send[7],&dev_id,2); - } + memcpy(&usart_send[9],&rx_buffer[DIST_IDX],4); usart_send[13] = battary; usart_send[14] = button; @@ -763,6 +760,7 @@ uint32_t frame_len; uint32_t resp_tx_time; uint8_t rec_nearbase_num,anc_report_num; +int16_t rec_antdelay; void Anchor_RecPoll(void) { tmp_time=TIM3->CNT; @@ -786,6 +784,8 @@ battary = rx_buffer[BATTARY_IDX]; button = rx_buffer[BUTTON_IDX]; + + frame_seq_nb2 = rx_buffer[SEQUENCE_IDX]; if(result==0) { @@ -836,10 +836,31 @@ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR); } } + double firstpath_power, rx_power,rec_firstpath_power; + double f1, f2, r1, r2; +uint16_t F1,F2,F3,N,C; +double B = 131072; +double A = 121.74; +double min_power; + dwt_rxdiag_t d1; +double LOS(dwt_rxdiag_t *dia) { + F1 = dia->firstPathAmp1; + F2 = dia->firstPathAmp2; + F3 = dia->firstPathAmp3; + N = dia->rxPreamCount; + C = dia->maxGrowthCIR; + + firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A; + rx_power=10*log10(C*B/(N*N))-A; + + // min_power = - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B)); + return min_power; + } +u16 smallcar_idlist[10]={0x4052,0x4032,0x4055,0x4034,0x4016,0x4010,0x4036,0x4027,0x4024,0x4047}; extern u16 dist_threshold; u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0; int32_t filter_dist,filter_speed; -u8 newmeasure; +u8 newmeasure,recpoll_len; u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base { u8 motorstate; @@ -855,18 +876,7 @@ 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);//接收超时时间 - if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1]) - { - motorstate =0; - }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2]) - { - motorstate =2; - }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3]) - { - motorstate =1; - }else{ - motorstate =0; - } + if(new_tagid) { @@ -875,19 +885,40 @@ }else{ memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4); } + motorstate =0; + + if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE1]||tagdist_list[taglist_pos]==0) + { + motorstate =0; + }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE2]) + { + if(flag_tag_distsmooth[taglist_pos]) + {motorstate =2; + }else{ + motorstate =0; + } + }else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3]) + { + if(flag_tag_distsmooth[taglist_pos]) + {motorstate =1; + }else{ + motorstate =0; + } + } tx_nearresp_msg[GROUP_ID_IDX] = group_id; tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase; tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE; - tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate; - if(remotesend_state) - { - memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,REMOTEPARA_LEN); - dwt_writetxdata(22+REMOTEPARA_LEN, tx_nearresp_msg, 0);//写入发送数据 - dwt_writetxfctrl(22+REMOTEPARA_LEN, 0);//设定发送长度 - }else{ - dwt_writetxdata(22, tx_nearresp_msg, 0);//写入发送数据 - dwt_writetxfctrl(22, 0);//设定发送长度 - } + tx_nearresp_msg[MOTORSTATE_INDEX]=motorstate;//(remotesend_state<<4)|motorstate; + tx_nearresp_msg[MOTORSTATE_INDEX]&=0x0f; +// if(remotesend_state) +// { +// memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,REMOTEPARA_LEN); +// dwt_writetxdata(22+REMOTEPARA_LEN, tx_nearresp_msg, 0);//写入发送数据 +// dwt_writetxfctrl(22+REMOTEPARA_LEN, 0);//设定发送长度 +// }else{ + dwt_writetxdata(24, tx_nearresp_msg, 0);//写入发送数据 + dwt_writetxfctrl(24, 0);//设定发送长度 + // } result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送,等待接收 @@ -915,6 +946,7 @@ uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32; double Ra, Rb, Da, Db; int64_t tof_dtu; + u8 si; resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3 final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6 final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1,T4,T5 @@ -932,7 +964,9 @@ tof = tof_dtu * DWT_TIME_UNITS; distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间 dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数 - dist_cm = dist_no_bias * 1000; //dis 为单位为cm的距离 + dist_cm = dist_no_bias * 1000; //dis 为单位为cm的距离 + dwt_readdiagnostics(&d1); + LOS(&d1); /*--------------------------以下为非测距逻辑------------------------*/ //dist_cm=33000; getrange_success = 1; @@ -940,9 +974,22 @@ LED0_BLINK; //每成功一次通讯则闪烁一次 dis_after_filter=dist_cm; hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10; + + if(recpoll_len==rec_nearbase_num*4+15) + { + hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10-rec_antdelay*10; + } + g_flag_Taggetdist[taglist_pos]=0; - if(hex_dist>-1000&&hex_dist<2000000) + + if(hex_dist>-100000&&hex_dist<2000000) { + if(abs(hex_dist-his_dist[taglist_pos])<10000) + { + flag_tag_distsmooth[taglist_pos] =1; + }else{ + flag_tag_distsmooth[taglist_pos] =0; + } if(abs(hex_dist-his_dist[taglist_pos])<dist_threshold||misdist_num[taglist_pos]>3) { @@ -959,7 +1006,12 @@ misdist_num[taglist_pos]=0; tagdist_list[taglist_pos] = filter_dist; his_dist[taglist_pos]=hex_dist; - g_Tagdist[taglist_pos]=filter_dist; + if(hex_dist>0) + { + g_Tagdist[taglist_pos]=hex_dist/10; + }else{ + g_Tagdist[taglist_pos]= 0x2ffff; + } #ifdef USART_SINGLE_OUTPUT usart_send[2] = 1;//正常模式 usart_send[3] = 17;//数据段长度 @@ -967,10 +1019,12 @@ memcpy(&usart_send[5],&tag_id_recv,2); memcpy(&usart_send[7],&dev_id,2); if(tagdist_list[taglist_pos]<=0) - tagdist_list[taglist_pos]=10; + tagdist_list[taglist_pos]=0x2ffff; 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; checksum = Checksum_u16(&usart_send[2],17); memcpy(&usart_send[19],&checksum,2); UART_PushFrame(usart_send,21); @@ -1020,6 +1074,7 @@ dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位 frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度 + recpoll_len = frame_len; dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据 memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2); //将收到的tag_id分别写入各次通讯的包中,为多标签通讯服务,防止一次通讯中接收到不同ID标签的数据 @@ -1081,6 +1136,7 @@ memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2); memcpy(&rec_ancidlist,&rx_buffer[NEARBASEID_INDEX],2*rec_nearbase_num); memcpy(&rec_ancdistlist,&rx_buffer[NEARBASEID_INDEX+2*rec_nearbase_num],2*rec_nearbase_num); + memcpy(&rec_antdelay,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*4],2); #ifdef USART_ALL_OUTPUT usart_send[2] = 0x0c;//正常模式 usart_send[3] = 8+rec_nearbase_num*4;//数据段长度 -- Gitblit v1.9.3