UWB_SmallModule_F103.git

			@@ -275,6 +275,10 @@
			tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
			tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;

			memcpy(&tx_poll_msg[GROUP_ID_IDX], &group_id, 1);
			memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
			memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);

			memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 4);
			memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
			memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4);
			@@ -374,7 +378,7 @@
			/* Check that the frame is the expected response from the companion "DS TWR responder" example.
			* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */

			if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			{ u16 anc_id_recv;
			/* Retrieve poll transmission and response reception timestamp. */
			poll_tx_ts = get_tx_timestamp_u64(); //è·å¾POLLåéæ¶é´T1
			@@ -422,8 +426,10 @@
			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;//æ°æ®æ®µé¿åº¦
			@@ -464,10 +470,10 @@
			// deca_sleep(10);
			}
			// dwt_entersleep();
			if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
			{
			//poll_timer +=time32_incr&0x7+3;
			}
			// if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
			// {
			// //poll_timer +=time32_incr&0x7+3;
			// }

			/* Execute a delay between ranging exchanges. */

			@@ -521,7 +527,7 @@
			// tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
			// }

			if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //å¤ææ¯å¦æ¯pollåæ°æ®
			if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //å¤ææ¯å¦æ¯pollåæ°æ®
			{
			tmp_time=TIM3->CNT;
			memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
			@@ -543,7 +549,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);//è®¾å®åéé¿åº¦
			@@ -575,7 +581,7 @@
			/* Check that the frame is a final message sent by "DS TWR initiator" example.
			* As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */

			if (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //å¤ææ¯å¦ä¸ºFinalå
			if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //å¤ææ¯å¦ä¸ºFinalå
			{
			uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
			uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
			@@ -615,29 +621,24 @@
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			g_UWB_com_interval = 0;
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
			if(hex_dist-anchor_dist_last_frm[tag_id_recv-TAG_ID_START]<15000\|\|misdist_num>4)

			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
			if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
			{
			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)
			{
			if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
			{
			if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<10000)
			{
			g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
			anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;

			{
			#ifdef TDFILTER
			TrackingDiffUpdate(pUWBDistanceTrackingDiff, (float)hex_dist);
			filter_dist=pUWBDistanceTrackingDiff->pos_predict/10;
			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]= anchor_dist_last_frm[tag_id_recv-TAG_ID_START];
			}
			g_Tagdist[tag_id_recv]= filter_dist;

			his_dist[tag_id_recv-TAG_ID_START]=hex_dist;
			g_flag_Taggetdist[tag_id_recv]=0;
			if(!g_com_map[MODBUS_MODE])
			@@ -656,16 +657,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++;
			}
			if(anchor_dist_last_frm[tag_id_recv-TAG_ID_START]>1000)
			{anchor_dist_last_frm[tag_id_recv-TAG_ID_START]=anchor_dist_last_frm[tag_id_recv-TAG_ID_START];}
			}
			}
			}else{
			/* Clear RX error events in the DW1000 status register. */