UWB_SmallModule_F103.git

			@@ -1,5 +1,7 @@
			#include "dw_app.h"
			#include "ADC.h"
			#include "modbus.h"
			#define TDFILTER
			enum enumtagstate
			{
			DISCPOLL,
			@@ -9,14 +11,14 @@
			static dwt_config_t config = {
			2, /* Channel number. */
			DWT_PRF_64M, /* Pulse repetition frequency. */
			DWT_PLEN_128, /* Preamble length. */
			DWT_PLEN_1024, /* Preamble length. */
			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. */
			(1025 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
			};
			static uint8_t tx_poll_msg[20] = {0};
			static uint8_t tx_sync_msg[14] = {0};
			@@ -47,7 +49,7 @@
			uint32_t g_UWB_com_interval = 0;
			float dis_after_filter; //å½åè·ç¦»å¼
			LPFilter_Frac* p_Dis_Filter; //æµè·ç¨çä½éæ»¤æ³¢å¨
			uint16_t g_Tagdist[TAG_NUM_IN_SYS];
			int32_t g_Tagdist[TAG_NUM_IN_SYS];
			uint8_t g_flag_Taggetdist[256];

			static uint64_t get_tx_timestamp_u64(void)
			@@ -109,11 +111,20 @@
			g_flag_Taggetdist[i]++;
			if(g_flag_Taggetdist[i]>=2)
			{
			g_Tagdist[i]=0xffff;
			g_Tagdist[i]=0x1ffff;
			}
			}
			}
			}
			void SetNLOSNTMSettings(void)
			{
			uint8_t temp;
			temp = 7;
			dwt_writetodevice(LDE_IF_ID,LDE_CFG1_OFFSET,1,&temp);
			dwt_write16bitoffsetreg(LDE_IF_ID,LDE_CFG2_OFFSET,3);
			// NTM = dwt_read32bitoffsetreg(LDE_IF_ID,LDE_CFG1_OFFSET) & LDE_CFG1_NSTDEV_MASK;
			}
			extern int16_t g_commap_antdelay;
			void Dw1000_Init(void)
			{
			/* Reset and initialise DW1000.
			@@ -126,16 +137,18 @@
			/* Configure DW1000. See NOTE 6 below. */
			dwt_configure(&config);//éç½®DW1000


			// SetNLOSNTMSettings();

			/* 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. */
			//è®¾ç½®æ¥æ¶è¶æ¶æ¶é´
			}

			extern u16 group_id;
			void Dw1000_App_Init(void)
			{
			//g_com_map[DEV_ID] = 0x0b;
			@@ -144,6 +157,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, 2);
			memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
			memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
			@@ -332,15 +349,25 @@
			last_nearbase_num=next_nearbase_num;
			nearbase_num=next_nearbase_num;
			recbase_num=0;
			tx_nearpoll_msg[GROUP_ID_IDX] = group_id;
			tx_nearpoll_msg[BATTARY_IDX] = Get_Battary();
			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_num2+i2],&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+2*nearbase_num, tx_nearpoll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(13+2*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;
			@@ -382,7 +409,7 @@
			dwt_readrxdata(rx_buffer, frame_len, 0); //è¯»åæ¥æ¶æ°æ®
			dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			dwt_rxenable(0);//æå¼æ¥æ¶
			if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX] == group_id) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			{ u16 rec_nearbaseid,rec_nearbasepos;
			int32_t temp_dist;
			poll_tx_ts = get_tx_timestamp_u64(); //è·å¾POLLåéæ¶é´T1
			@@ -443,23 +470,23 @@
			final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX], resp_rx_ts);
			final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
			tx_nearfinal_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
			tx_nearfinal_msg[GROUP_ID_IDX] = group_id;
			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);
			IWDG_Feed();
			if(temp_dist!=0x1ffff)
			{
			#ifndef USART_INTEGRATE_OUTPUT
			g_Resttimer=0;

			#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;
			@@ -498,18 +525,14 @@
			// tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
			if(temp_dist!=0x1ffff)
			{
			#ifndef USART_INTEGRATE_OUTPUT
			#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;
			@@ -605,10 +628,7 @@
			report_num++;
			}
			}
			for(i=0;i<MAX_NEARBASE_NUM;i++)
			{
			nearbase_distlist[i]=0x1ffff;
			}


			// printf("%d,%d",temp_sync_timer2,temp_sync_timer1);
			#ifdef USART_INTEGRATE_OUTPUT
			@@ -699,7 +719,6 @@
			{
			//LED0_ON;
			dwt_forcetrxoff();
			g_Resttimer=0;
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			switch(tag_state)
			{
			@@ -722,9 +741,9 @@
			int8_t correction_time,new_tagid=0;
			extern uint8_t sync_seq;
			u16 taglist_num=0,taglist_pos;
			u16 tagid_list[TAG_NUM_IN_SYS];
			u16 tagid_list[TAG_NUM_IN_SYS],tagstate_list[TAG_NUM_IN_SYS];;
			u8 tagofflinetime[TAG_NUM_IN_SYS];
			int32_t tagdist_list[TAG_NUM_IN_SYS];
			int32_t tagdist_list[TAG_NUM_IN_SYS],tag_histdist_list[TAG_NUM_IN_SYS];
			void TagListUpdate(void)
			{
			u16 i,j=0,temp[TAG_NUM_IN_SYS];
			@@ -750,6 +769,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;
			@@ -773,6 +793,8 @@

			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];


			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			if(result==0)
			{
			@@ -823,35 +845,60 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor;
			u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			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((F1F1+F2F2+F3F3)/(NN))-A;
			rx_power=10log10(CB/(N*N))-A;

			// min_power = - 10 * log10((F1 F1 + F2 F2 + F3 * F3) / (C *B));
			return min_power;
			}
			static void resp_msg_set_ts(uint8_t *ts_field, const uint64_t ts)
			{
			u8 motorstate;
			tmp_time=TIM3->CNT;
			int i;
			for (i = 0; i < 4; i++)
			{
			ts_field[i] = (ts >> (i * 8)) & 0xFF;
			}
			}
			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,recpoll_len;
			uint8_t temp_nearbaspos;
			float fiter_p;
			uint8_t Anchor_RecNearPoll(uint8_t ancrec_nearbasepos) //0 mainbase 1 first near_base
			{
			uint8_t motorstate;
			// tmp_time=TIM3->CNT;
			temp_nearbaspos = ancrec_nearbasepos;
			memcpy(&tx_nearresp_msg[ANCTIMEMS],&sync_timer,2);
			memcpy(&tx_nearresp_msg[ANCTIMEUS],&tmp_time,2);
			memcpy(&tx_nearresp_msg[TAGSLOTPOS],&taglist_pos,2);
			memcpy(&tx_nearresp_msg[TAG_ID_IDX],&tag_id_recv,2);
			memcpy(&tx_nearresp_msg[RESP_MSG_ANC_DISTOFFSET],&g_com_map[DIST_OFFSET],2);
			poll_rx_ts = get_rx_timestamp_u64();//è·å¾Pollåæ¥æ¶æ¶é´T2

			resp_tx_time = (poll_rx_ts + ((POLL_RX_TO_RESP_TX_DLY_UUS+ancrec_nearbaseposDELAY_BETWEEN_TWO_FRAME_UUS) UUS_TO_DWT_TIME)) >> 8;//è®¡ç®Responseåéæ¶é´T3ã
			dwt_setdelayedtrxtime(resp_tx_time);//è®¾ç½®Responseåéæ¶é´T3
			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;
			}

			dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//æ¥æ¶è¶æ¶æ¶é´ã
			resp_tx_ts = (((uint64_t)(resp_tx_time & 0xFFFFFFFEUL)) << 8) + TX_ANT_DLY;
			resp_msg_set_ts(&tx_nearresp_msg[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
			resp_msg_set_ts(&tx_nearresp_msg[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
			if(new_tagid)
			{
			tagdist_list[taglist_pos]=0x1ffff;
			@@ -859,117 +906,96 @@
			}else{
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}
			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)
			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])
			{
			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);//è®¾å®åéé¿åº¦
			}
			result = dwt_starttx(DWT_START_TX_DELAYED \| DWT_RESPONSE_EXPECTED);//å»¶è¿åéï¼çå¾æ¥æ¶

			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			if(result==0)
			if(1)//flag_tag_distsmooth[taglist_pos])
			{motorstate =2;
			}else{
			motorstate =0;
			}
			}else if(tagdist_list[taglist_pos]<g_com_map[ALARM_DISTANCE3])
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{ };
			}else{
			result++;
			}

			if (status_reg & SYS_STATUS_RXFCG)//æ¥æ¶æå
			{

			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG \| SYS_STATUS_TXFRS);//æ¸æ¥æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;//æ°æ®é¿åº¦
			dwt_readrxdata(rx_buffer, frame_len, 0);//è¯»åæ¥æ¶æ°æ®
			if(seize_anchor&&memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,2)) //æ¢å anchor å¤±è´¥
			{
			return 1;
			}
			if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //å¤ææ¯å¦ä¸º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;
			double Ra, Rb, Da, Db;
			int64_t tof_dtu;
			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
			final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_NEARBASE_IDX+ancrec_nearbasepos*4], &resp_rx_ts);
			final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

			poll_rx_ts_32 = (uint32_t)poll_rx_ts;//ä½¿ç¨32ä½æ°æ®è®¡ç®
			resp_tx_ts_32 = (uint32_t)resp_tx_ts;
			final_rx_ts_32 = (uint32_t)final_rx_ts;
			Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
			Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
			Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
			Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);//Treply1 = T3 - T2
			tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//è®¡ç®å¬å¼
			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 * 100; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			//dist_cm=33000;

			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
			g_flag_Taggetdist[taglist_pos]=0;
			if(hex_dist>-1000&&hex_dist<200000)
			{
			if(abs(hex_dist-his_dist[taglist_pos])<1500\|\|misdist_num[taglist_pos]>3)
			{
			misdist_num[taglist_pos]=0;
			tagdist_list[taglist_pos] = hex_dist;
			his_dist[taglist_pos]=hex_dist;
			g_Tagdist[taglist_pos]=hex_dist;
			#ifndef USART_INTEGRATE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[5],&tag_id_recv,2);
			memcpy(&usart_send[7],&dev_id,2);
			memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
			usart_send[13] = battary;
			usart_send[14] = button;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			#else
			memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);
			memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);
			anc_report_num++;
			#endif
			if(1)//flag_tag_distsmooth[taglist_pos])
			{motorstate =1;
			}else{
			// printf("%d",hex_dist);
			misdist_num[taglist_pos]++;
			}
			motorstate =0;
			}
			}
			}else{
			//printf("%x/n",status_reg);
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			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]=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(38, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(38, 0);//è®¾å®åéé¿åº¦
			// }
			result = dwt_starttx(DWT_START_TX_DELAYED );//å»¶è¿åéï¼çå¾æ¥æ¶


			if(result==0)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_TXFRS ))&&!g_start_sync_flag)///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{ };
			}else{
			// if(g_com_map[MODBUS_ID1]==1)
			// {
			// #ifdef DBGMSG_OUTPUT
			// dbgmsg_len = sprintf(dbgmsg,"%s status_reg:%x",__debug_info__,status_reg);
			// SendDbgMsg(dbgmsg,dbgmsg_len);
			// #endif
			// }
			result++;
			}
			if(!seize_anchor)
			{
			if(abs(tagdist_list[taglist_pos]-tag_histdist_list[taglist_pos])<g_com_map[SPEEDFILTER_THRES]\|\|misdist_num[taglist_pos]>3)
			{
			if(tag_histdist_list[taglist_pos]>0&&tag_histdist_list[taglist_pos]<50000)
			{
			filter_dist=tagdist_list[taglist_pos]fiter_p+(1-fiter_p)tag_histdist_list[taglist_pos];
			}else{
			filter_dist = tagdist_list[taglist_pos];
			}
			tag_histdist_list[taglist_pos] = filter_dist;
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[5],&tag_id_recv,2);
			memcpy(&usart_send[7],&dev_id,2);
			memcpy(&usart_send[9],&tag_histdist_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);
			}else{
			misdist_num[taglist_pos]++;
			}
			}

			}
			uint32_t current_syncid=0xffffffff,synclost_timer;
			extern u8 flag_syncbase;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num,getrange_success=0;
			u16 ancidlist_rec[20],ancidlist_send[20];
			extern uint8_t flag_syncbase;
			uint8_t tagpos_rec[50],tagpos_send[50],ancidlist_num;
			uint16_t ancidlist_rec[20],ancidlist_send[20],rec_ancidlist[20],rec_ancdistlist[20];
			void Anchor_App(void)
			{

			u8 send_len,i;
			u16 tempid;
			uint8_t send_len,i;
			uint16_t tempid;
			uint32_t rec_syncid;

			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			@@ -985,31 +1011,33 @@
			};
			//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			if (status_reg & SYS_STATUS_RXFCG)//æåæ¥æ¶
			{ u16 tag_recv_interval;
			{ uint16_t tag_recv_interval;
			float temp_tagpos;

			getrange_success = 1;
			g_Resttimer=0;

			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æ ç¾çæ°æ®
			memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
			if(group_id==rx_buffer[GROUP_ID_IDX])
			{
			switch(rx_buffer[MESSAGE_TYPE_IDX])
			{
			// case POLL:
			// if (anchor_type == rx_buffer[ANC_TYPE_IDX])
			// Anchor_RecPoll();
			// break;
			case SYNC:
			case SYNC:
			memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
			if(rec_syncid<current_syncid)
			{
			current_syncid=rec_syncid;
			flag_syncbase=0;
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+15;
			// TIM3->CNT = sync_seq*325%1000+15;
			sync_timer = sync_seq*325/1000;
			synclost_timer=0;
			//SyncPoll(sync_seq,rec_syncid);
			@@ -1019,7 +1047,7 @@
			{
			flag_syncbase=0;
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+5;
			// TIM3->CNT = sync_seq*325%1000+5;
			sync_timer = sync_seq*325/1000+995;
			synclost_timer=0;
			//SyncPoll(sync_seq,rec_syncid);
			@@ -1043,8 +1071,32 @@
			}
			break;

			case NEAR_POLL:
			case NEAR_POLL:
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			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+2rec_nearbase_num],2rec_nearbase_num);
			memcpy(&rec_antdelay,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*4],2);
			g_Resttimer = 0;
			#ifdef USART_ALL_OUTPUT
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 8+rec_nearbase_num*4;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb2;//æ°æ®æ®µé¿åº¦
			usart_send[5] = battary;
			usart_send[6] = button;
			usart_send[7] = rec_nearbase_num;
			memcpy(&usart_send[8],&tag_id_recv,2);
			memcpy(&usart_send[10],&rec_ancidlist,2*rec_nearbase_num);
			memcpy(&usart_send[10+rec_nearbase_num2],&rec_ancdistlist,2rec_nearbase_num);
			checksum = Checksum_u16(&usart_send[2],8+4*rec_nearbase_num);
			memcpy(&usart_send[10+4*rec_nearbase_num],&checksum,2);
			UART_PushFrame(usart_send,12+rec_nearbase_num*4);
			#endif
			if(tag_id_recv>=g_com_map[WHITELIST1_START]&&tag_id_recv<=g_com_map[WHITELIST1_END])
			{break;}
			taglist_pos=CmpTagInList(tag_id_recv);
			if(taglist_pos==taglist_num)
			{
			@@ -1056,9 +1108,9 @@
			new_tagid=0;
			}
			tagofflinetime[taglist_pos]=0;
			temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
			tagpos_rec[(u8)temp_tagpos]=1;
			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
			tagpos_rec[(uint8_t)temp_tagpos]=1;


			if(rec_nearbase_num>ancidlist_num)
			{
			@@ -1072,6 +1124,11 @@
			if(tempid==dev_id)
			{
			seize_anchor=0; //éæ¢å ãå·²åå¨åè¡¨ä¸
			if(rec_ancdistlist[i]!=0)
			{
			g_Tagdist[taglist_pos] = rec_ancdistlist[i];
			tagdist_list[taglist_pos] = rec_ancdistlist[i];
			}
			Anchor_RecNearPoll(i);
			break;
			}
			@@ -1085,8 +1142,17 @@

			break;
			}
			}else{
			}
			}else{
			// if(g_com_map[MODBUS_ID1]==1)
			// {
			// #ifdef DBGMSG_OUTPUT
			// dbgmsg_len = sprintf(dbgmsg,"%s status_reg:%x",__debug_info__,status_reg);
			// SendDbgMsg(dbgmsg,dbgmsg_len);
			// #endif
			// }
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			dwt_forcetrxoff();
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}