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