From affb754b56216235932464dbaffbaf34638cd8f2 Mon Sep 17 00:00:00 2001
From: zhyinch <zhyinch@gmail.com>
Date: 星期四, 30 一月 2020 10:02:15 +0800
Subject: [PATCH] 增加标签第一个数据不输出

---
 源码/核心板/Src/application/dw_app.c |   66 ++++++++++++++++++++++-----------
 1 files changed, 44 insertions(+), 22 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 d017aa1..1268afd 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"
@@ -94,7 +94,7 @@
 	DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
 	9,               /* TX preamble code. Used in TX only. */
 	9,               /* RX preamble code. Used in RX only. */
-	0,               /* Use non-standard SFD (Boolean) */
+	1,               /* Use non-standard SFD (Boolean) */
 	DWT_BR_6M8,      /* Data rate. */
 	DWT_PHRMODE_STD, /* PHY header mode. */
 	(129 + 8 - 8)    /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
@@ -238,7 +238,7 @@
 		for(i=0;i<255;i++)
 		{
 			g_flag_Taggetdist[i]++;
-			if(g_flag_Taggetdist[i]>=2)
+			if(g_flag_Taggetdist[i]>=20)
 			{
 				g_Tagdist[i]=0xffff;
 			}
@@ -313,7 +313,7 @@
 uint16_t g_Resttimer;
 uint8_t result;
 u8 tag_succ_times=0;
-int32_t hex_dist;
+int32_t hex_dist,hex_dist2;
 u16 checksum;
 int8_t tag_delaytime;
 extern uint16_t sync_timer;
@@ -395,7 +395,7 @@
 			}
 			TIM3->CNT=tmp_time;
 		}
-			memcpy(&anchor_dist_last_frm[0], &rx_buffer[DIST_IDX], 4);
+			memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
 			memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
 			/* Compute final message transmission time. See NOTE 9 below. */
 			final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间,T5=T4+Treply2
@@ -418,19 +418,21 @@
 			tag_succ_times++;
 			
 				LED0_BLINK;
-				hex_dist = anchor_dist_last_frm[0];
+				
 					memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
 //					g_Tagdist[anc_id_recv]=	hex_dist;
 //					g_flag_Taggetdist[anc_id_recv]=0;
-					if(!g_com_map[MODBUS_MODE])
+					
+					if(!g_com_map[MODBUS_MODE]&&hex_dist2!=0xffff)
 					{
+					hex_dist2 = hex_dist2+(int16_t)g_com_map[DIST_OFFSET];		
 					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],&rx_buffer[ANCHOR_ID_IDX],2);
 				
-					memcpy(&usart_send[9],&hex_dist,4);
+					memcpy(&usart_send[9],&hex_dist2,4);
 					usart_send[13] = battary;
 					usart_send[14] = button;
 					checksum = Checksum_u16(&usart_send[2],17);
@@ -474,12 +476,14 @@
 }
 int8_t correction_time;
 extern uint8_t sync_seq;
-
+#define TDFILTER
+//#define CHECK_UID
+extern uint8_t UID_ERROR;
 void Anchor_App(void)
 {
 	uint32_t frame_len;
 	uint32_t resp_tx_time;
-	
+	static u8 misdist_num;
 	/* Clear reception timeout to start next ranging process. */
 	dwt_setrxtimeout(0);//设定接收超时时间,0位没有超时时间
 
@@ -541,7 +545,7 @@
 
 			/* Write and send the response message. See NOTE 9 below.*/
 			if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
-			memcpy(&tx_resp_msg[DIST_IDX], &anchor_dist_last_frm[tag_id_recv-TAG_ID_START], 4);
+			memcpy(&tx_resp_msg[DIST_IDX], &g_Tagdist[tag_id_recv], 4);
 		
 			dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
 			dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
@@ -589,6 +593,10 @@
 					final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
 					final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
 
+					#ifdef CHECK_UID
+					if(UID_ERROR==1)
+						poll_rx_ts=0;
+					#endif
 					/* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
 					poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
 					resp_tx_ts_32 = (uint32_t)resp_tx_ts;
@@ -603,22 +611,31 @@
 					distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
 					dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
 
-					dist_cm = dist_no_bias * 100; //dis 为单位为cm的距离
+					dist_cm = dist_no_bias * 1000; //dis 为单位为cm的距离
 //					dist[TAG_ID] = LP(dis, TAG_ID); //LP 为低通滤波器,让数据更稳定
 					
 					/*--------------------------以下为非测距逻辑------------------------*/
 					LED0_BLINK; //每成功一次通讯则闪烁一次
 					g_UWB_com_interval = 0;
-					dis_after_filter=dist_cm;
-					hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
-						if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
-					if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<1000&&hex_dist<100000&&hex_dist>-1000)
+					
+					hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
+					if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
 					{
-						g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
-						anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
-					}
+					if(hex_dist-his_dist[tag_id_recv-TAG_ID_START]<15000||misdist_num>4)
+					{int32_t filter_dist;
+						misdist_num=0;
+					if(hex_dist<1000000&&hex_dist>-10000)
+					{		
+						#ifdef TDFILTER
+						NewTrackingDiffUpdate(tag_id_recv-TAG_ID_START, (float)hex_dist);
+						filter_dist=pos_predict[tag_id_recv-TAG_ID_START]/10;
+						#else
+						filter_dist=hex_dist/10;
+						#endif
+						anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=filter_dist;
+						g_Tagdist[tag_id_recv]=	filter_dist;
+					
 					his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
-					g_Tagdist[tag_id_recv]=	anchor_dist_last_frm[tag_id_recv-TAG_ID_START];
 					g_flag_Taggetdist[tag_id_recv]=0;
 					if(!g_com_map[MODBUS_MODE])
 					{
@@ -636,10 +653,15 @@
 						UART_PushFrame(usart_send,21);
 					}	
 					//	memcpy(&Modbus_HoldReg[tag_id_recv*2],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
-					Modbus_HoldReg[tag_id_recv*2]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START]>>16;
-					Modbus_HoldReg[tag_id_recv*2+1]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START];
+					Modbus_HoldReg[tag_id_recv*2]=g_Tagdist[tag_id_recv-TAG_ID_START]>>16;
+					Modbus_HoldReg[tag_id_recv*2+1]=g_Tagdist[tag_id_recv-TAG_ID_START];
 					//dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
-
+						}
+					
+					}else{
+						misdist_num++;
+					}
+				}
 				}
 			}else{
 				/* Clear RX error events in the DW1000 status register. */

--
Gitblit v1.9.3