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 | 116 +++++++++++++++++++++++++++++++++++++-------------------- 1 files changed, 75 insertions(+), 41 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 7c3db08..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" @@ -49,7 +49,7 @@ float dis_after_filter; //当前距离值 LPFilter_Frac* p_Dis_Filter; //测距用的低通滤波器 int32_t g_Tagdist[TAG_NUM_IN_SYS]; -uint8_t g_flag_Taggetdist[256]; +uint8_t g_flag_Taggetdist[256],flag_tag_distsmooth[TAG_NUM_IN_SYS]; static uint64_t get_tx_timestamp_u64(void) { @@ -344,6 +344,7 @@ 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);} @@ -437,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); @@ -464,7 +466,7 @@ dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//将发送数据写入DW1000 dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度 memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4); - if(temp_dist!=0x1ffff&&g_com_map[ALARM_DISTANCE1] == 1) + if(temp_dist!=0x1ffff) { g_Resttimer=0; IWDG_Feed(); @@ -472,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; @@ -516,20 +514,16 @@ LED0_BLINK; // memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4); // tagslotpos=GetRandomSlotPos(rec_tagpos_binary); - if(temp_dist!=0x1ffff&&g_com_map[ALARM_DISTANCE1] == 1) + if(temp_dist!=0x1ffff) { #ifdef USART_SINGLE_OUTPUT 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; @@ -842,6 +836,26 @@ 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; @@ -862,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) { @@ -882,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);//延迟发送,等待接收 @@ -940,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; @@ -958,6 +984,12 @@ 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) { @@ -978,7 +1010,7 @@ { g_Tagdist[taglist_pos]=hex_dist/10; }else{ - g_Tagdist[taglist_pos]= 1; + g_Tagdist[taglist_pos]= 0x2ffff; } #ifdef USART_SINGLE_OUTPUT usart_send[2] = 1;//正常模式 @@ -987,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); @@ -1102,7 +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+nearbase_num*4],2); + 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