UWB_SmallModule_F103.git

			@@ -9,11 +9,11 @@
			static dwt_config_t config = {
			2, /* Channel number. */
			DWT_PRF_64M, /* Pulse repetition frequency. */
			DWT_PLEN_128, /* Preamble length. */
			DWT_PLEN_64, /* 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. */
			@@ -38,7 +38,7 @@
			static uint64_t resp_tx_ts;
			static uint64_t final_rx_ts;
			static double tof;
			int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS]; ;
			int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];
			uint32_t tag_id = 0;
			uint32_t tag_id_recv = 0;
			uint32_t anc_id_recv = 0;
			@@ -47,7 +47,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)
			@@ -97,6 +97,7 @@
			ts += ts_field[i] << (i 8);
			}
			}
			extern int32_t tagdist_list[TAG_NUM_IN_SYS];
			void TagDistClear(void)
			{
			static uint16_t clear_judge_cnt;
			@@ -109,7 +110,7 @@
			g_flag_Taggetdist[i]++;
			if(g_flag_Taggetdist[i]>=2)
			{
			g_Tagdist[i]=0xffff;
			tagdist_list[i]=0x1ffff;
			}
			}
			}
			@@ -165,8 +166,8 @@
			}

			u16 tag_time_recv[TAG_NUM_IN_SYS];
			u8 usart_send[100],usart_send_anc[100];
			u8 battary,button;
			u8 usart_send[150],usart_send_anc[100];
			u8 battary,button,tag_frequency,tag_slotpos;
			extern uint8_t g_pairstart;
			void tag_sleep_configuraion(void)
			{
			@@ -176,14 +177,14 @@

			extern uint8_t g_start_send_flag;
			u8 g_start_sync_flag;
			void SyncPoll(u8 sync_seq,uint32_t sync_baseid)
			void SyncPoll(u8 sync_seq)
			{u8 result;
			g_start_sync_flag=1; //ä¸ææ¨¡å¼ï¼éåºç»ç«¯åï¼éè¦éæ°æ¥è¿
			dwt_forcetrxoff(); //å³éæ¥æ¶ï¼ä»¥é²å¨RX ON ç¶æ


			tx_sync_msg[SYNC_SEQ_IDX]=sync_seq;
			memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&sync_baseid,4);
			memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&dev_id,4);
			dwt_writetxdata(sizeof(tx_sync_msg), tx_sync_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE);
			@@ -309,11 +310,30 @@
			}
			return max_slotpos-1;
			}
			dwt_rxdiag_t d1;
			u8 test=0;
			u8 anclost_times=0;
			u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
			u16 temp_sync_timer1,temp_sync_timer2;
			double firstpath_power, rx_power;
			double f1, f2, r1, r2;
			uint16_t F1,F2,F3,N,C;
			double B = 131072;
			double A = 121.74;
			double min_power;
			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;
			}
			void Poll(void)
			{
			uint32_t frame_len;
			@@ -506,7 +526,7 @@
			flag_rxon=1;
			neartimout_timer=0;
			get_newbase=0;
			timeout=ceil((float)nearbase_num*SLOT_SCALE)+3;
			timeout=ceil((float)nearbase_num*SLOT_SCALE)+2;
			//timeout=5;
			mainbase_dist=100000;
			mainbase_lost_count++;
			@@ -820,6 +840,7 @@
			}
			return i;
			}
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
			uint32_t frame_len;
			uint32_t resp_tx_time;
			uint8_t rec_nearbase_num,anc_report_num;
			@@ -844,8 +865,7 @@
			dwt_writetxfctrl(sizeof(tx_resp_msg), 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)
			{
			@@ -886,46 +906,70 @@
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
			if(abs(hex_dist-his_dist[taglist_pos])<1000)
			{
			tagdist_list[taglist_pos] = hex_dist;
			}
			his_dist[taglist_pos]=hex_dist;
			if(hex_dist>-1000&&hex_dist<200000)
			{
			g_flag_Taggetdist[taglist_pos]=0;
			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;
			usart_send[15] = firstpath_power;
			usart_send[16] = (rx_power-firstpath_power)*10;
			usart_send[17] = tag_frequency;
			usart_send[18] = tag_slotpos;
			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
			#ifdef DEBUG_INF
			printf("åºå·ï¼%d\r\n",frame_seq_nb2);
			#endif
			}else{
			// printf("%d",hex_dist);
			misdist_num[taglist_pos]++;
			}
			}
			}
			}else{
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			uint32_t time_monitor[10];
			extern uint16_t configremotetagID;
			extern u8 remotetag_paralen;
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse;
			u8 finalsend=0;

			u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			{
			u8 motorstate;

			tmp_time=TIM3->CNT;
			memcpy(&tx_nearresp_msg[NR_NEARSWITCH_DISTANCE],&g_com_map[NEARSWITCH_DISTANCE_INDEX],2);
			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);
			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ã
			resp_tx_time = (poll_rx_ts + ((POLL_RX_TO_RESP_TX_DLY_UUS+(ancrec_nearbasepos)DELAY_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_readdiagnostics(&d1);

			if(new_tagid)
			{
			@@ -938,6 +982,7 @@
			tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
			tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)\|motorstate;
			waittagconfig_reponse=0;
			finalsend = 0;
			if(pwtag.remain_time>0)
			{uint8_t i;
			for(i=0;i<pwtag.groupnum;i++)
			@@ -948,20 +993,23 @@
			tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)\|motorstate;
			remotetag_paralen = 2+3;
			remotetag_para[0] = 2;
			remotetag_para[1] = 6;
			remotetag_para[1] = pwtag.index;
			remotetag_para[2] = 2;
			memcpy(&remotetag_para[3],&pwtag.group_interval[i],2);
			memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,remotetag_paralen);
			dwt_writetxdata(22+remotetag_paralen, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(22+remotetag_paralen, 0);//è®¾å®åéé¿åº¦
			dwt_writetxdata(24+remotetag_paralen, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(24+remotetag_paralen, 0);//è®¾å®åéé¿åº¦
			remotesend_state=0;
			finalsend = 1;
			break;
			}
			}
			}

			}else{
			dwt_writetxdata(22, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(22, 0);//è®¾å®åéé¿åº¦
			if(finalsend == 0)
			{
			dwt_writetxdata(24, tx_nearresp_msg, 0);//åå¥åéæ°æ®
			dwt_writetxfctrl(24, 0);//è®¾å®åéé¿åº¦
			}

			// if(remotesend_state&&tag_id_recv==configremotetagID)
			@@ -974,9 +1022,6 @@

			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)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_sync_flag)///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			@@ -1003,6 +1048,10 @@
			int64_t tof_dtu;
			resp_tx_ts = get_tx_timestamp_u64();//è·å¾responseåéæ¶é´T3
			final_rx_ts = get_rx_timestamp_u64();//è·å¾finalæ¥æ¶æ¶é´T6

			// dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			// dwt_rxenable(0);//æå¼æ¥æ¶

			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);
			@@ -1010,6 +1059,7 @@
			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;
			time_monitor[1] = sync_timer*1000+TIM3->CNT;
			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
			@@ -1018,16 +1068,21 @@
			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 = dist_no_bias * 100; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			dwt_readdiagnostics(&d1);
			// time_monitor[2] = sync_timer*1000+TIM3->CNT;
			LOS(&d1);
			time_monitor[3] = sync_timer*1000+TIM3->CNT;
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			//dist_cm=33000;

			GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
			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)
			{
			g_flag_Taggetdist[taglist_pos]=0;
			if(abs(hex_dist-his_dist[taglist_pos])<1500\|\|misdist_num[taglist_pos]>3)
			{
			misdist_num[taglist_pos]=0;
			@@ -1043,9 +1098,13 @@
			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-firstpath_power)*10;
			usart_send[17] = tag_frequency;
			usart_send[18] = tag_slotpos;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			// 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);
			@@ -1059,9 +1118,9 @@
			misdist_num[taglist_pos]++;
			}
			}
			if(rx_buffer[TAGCONFIGSUCCESS_INDEX]==1&&waittagconfig_reponse)
			if(rx_buffer[TAGCONFIGSUCCESS_INDEX]==1)
			{
			waittagconfig_reponse = 0;

			remotesend_state = 0;
			usart_send[2] = 7;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 5;//æ°æ®æ®µé¿åº¦
			@@ -1079,10 +1138,15 @@
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			extern int32_t intheight;
			extern u16 synclost_count;
			uint32_t current_syncid=0xffffffff,synclost_timer;
			extern u8 flag_syncbase;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
			extern u8 flag_syncbase,waitsync_flag;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num,zero_matrix[100]={0};
			u16 ancidlist_rec[20],ancidlist_send[20];
			u16 recnearbaselist_id[30];
			int32_t recnearbaselist_dist[30];
			int16_t rec_tagheight;
			void Anchor_App(void)
			{

			@@ -1104,7 +1168,7 @@
			if (status_reg & SYS_STATUS_RXFCG)//æåæ¥æ¶
			{ u16 tag_recv_interval;
			float temp_tagpos;

			time_monitor[0] = sync_timer*1000+TIM3->CNT;
			g_Resttimer=0;
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//æ¸é¤æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//è·å¾æ¥æ¶æ°æ®é¿åº¦
			@@ -1114,34 +1178,71 @@
			memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
			switch(rx_buffer[MESSAGE_TYPE_IDX])
			{
			case POLL:
			if (anchor_type == rx_buffer[ANC_TYPE_IDX])
			uint16_t checksum;
			case SPOLL:
			//if (anchor_type == rx_buffer[ANC_TYPE_IDX])
			if(dev_id==anc_id_recv)
			{
			taglist_pos=CmpTagInList(tag_id_recv);
			if(taglist_pos==taglist_num)
			{
			taglist_pos=taglist_num;
			tagid_list[taglist_num++]=tag_id_recv;

			new_tagid=1;
			}else{
			new_tagid=0;
			}
			tagofflinetime[taglist_pos]=0;

			rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
			tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];

			////////////////åºç«æ±æ»æ¨¡å¼æ°æ®
			memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
			memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
			memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num2],rec_nearbase_num4);
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 15+8*(rec_nearbase_num);//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[4],&tag_id_recv,2);
			usart_send[6] = rx_buffer[SEQUENCE_IDX];
			usart_send[7] = rx_buffer[SEQUENCEH_IDX];
			usart_send[8] = battary;
			usart_send[9] = button;
			memcpy(&usart_send[10],&rec_tagheight,2);
			usart_send[12] = tag_frequency;
			usart_send[13] = tag_slotpos;
			usart_send[14] = 0;
			usart_send[15] = 0;
			usart_send[16] = rec_nearbase_num;

			memcpy(&usart_send[17],&recnearbaselist_id,2*rec_nearbase_num);
			memcpy(&usart_send[17+rec_nearbase_num2],&recnearbaselist_dist,4rec_nearbase_num);
			memcpy(&usart_send[17+rec_nearbase_num6],zero_matrix,2rec_nearbase_num);

			checksum = Checksum_u16(&usart_send[2],15+8*rec_nearbase_num);
			memcpy(&usart_send[17+8*rec_nearbase_num],&checksum,2);
			UART_PushFrame(usart_send,19+8*rec_nearbase_num);
			Anchor_RecPoll();
			}
			break;
			case SYNC:
			memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
			if(rec_syncid<current_syncid)
			{
			current_syncid=rec_syncid;
			if(anc_id_recv==g_com_map[SYNCBASEID])
			{
			synclost_count = 0;
			flag_syncbase=0;
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+15;
			sync_timer = sync_seq*325/1000;
			TIM3->CNT = (sync_seq*325%1000)-80;
			sync_timer = (sync_seq*325/1000);
			synclost_timer=0;
			//SyncPoll(sync_seq,rec_syncid);
			}else if(rec_syncid==current_syncid)
			{
			if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)
			{
			flag_syncbase=0;
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+5;
			sync_timer = sync_seq*325/1000+995;
			synclost_timer=0;
			//SyncPoll(sync_seq,rec_syncid);
			}

			}
			if(g_com_map[BASESYNCSEQ]==2)
			SyncPoll(sync_seq);
			}
			break;
			case NEAR_MSG:
			if(anc_id_recv==dev_id)
			@@ -1153,14 +1254,53 @@
			ancidlist_num=g_com_map[NEARBASE_NUM];
			tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
			memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
			send_len=11+ancidlist_num*2+2;
			send_len=21+ancidlist_num*2;
			dwt_writetxdata(send_len, tx_near_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(send_len, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE);
			}
			break;
			case REG_POLL:
			for(i=0;i<MAX_REGTAGNUM;i++)
			{
			if(tag_id_recv==regtag_map.tagid[i]&&regtag_map.remain_time[i]>0)
			{
			tx_near_msg[REGR_TAGSLOTPOS_INDEX]=regtag_map.tag_slotpos[i];
			tx_near_msg[REGR_TAGFREQ_INDEX] = regtag_map.tag_frequency[i];
			memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
			memcpy(&tx_near_msg[ANCHOR_ID_IDX],&dev_id,2);
			tx_near_msg[MESSAGE_TYPE_IDX] = REG_RESPONSE;
			ancidlist_num=g_com_map[NEARBASE_NUM];
			tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
			memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
			send_len=21+ancidlist_num*2;
			dwt_writetxdata(send_len, tx_near_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(send_len, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE);
			regtag_map.tagid[i] = 0;
			break;
			}

			case NEAR_POLL:
			}
			if(i==MAX_REGTAGNUM)
			{
			dwt_readdiagnostics(&d1);
			LOS(&d1);
			usart_send[2]=0x0a;
			usart_send[3]=20;
			memcpy(&usart_send[4],&dev_id,2);
			memcpy(&usart_send[6],&tag_id_recv,2);
			memcpy(&usart_send[8],&rx_buffer[10],9);
			usart_send[17] = firstpath_power;
			memcpy(&usart_send[18],&rx_buffer[19],2);
			checksum = Checksum_u16(&usart_send[2],20);
			memcpy(&usart_send[22],&checksum,2);
			UART_PushFrame(usart_send,24);
			}
			break;
			case NEAR_POLL:
			// GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);

			memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
			taglist_pos=CmpTagInList(tag_id_recv);
			if(taglist_pos==taglist_num)
			@@ -1173,14 +1313,47 @@
			new_tagid=0;
			}
			tagofflinetime[taglist_pos]=0;
			temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
			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];

			battary = rx_buffer[BATTARY_IDX];
			button = rx_buffer[BUTTON_IDX];
			frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
			tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
			tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
			////////////////åºç«æ±æ»æ¨¡å¼æ°æ®
			memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],2);
			memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
			memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num2],rec_nearbase_num4+4);
			usart_send[2] = 0x0c;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 15+8*(rec_nearbase_num+1);//æ°æ®æ®µé¿åº¦
			memcpy(&usart_send[4],&tag_id_recv,2);
			usart_send[6] = rx_buffer[SEQUENCE_IDX];
			usart_send[7] = rx_buffer[SEQUENCEH_IDX];
			usart_send[8] = battary;
			usart_send[9] = button;

			memcpy(&usart_send[10],&rec_tagheight,2);
			usart_send[12] = tag_frequency;
			usart_send[13] = tag_slotpos;
			usart_send[14] = 0;
			usart_send[15] = 0;
			usart_send[16] = rec_nearbase_num+1;
			memcpy(&usart_send[17],&anc_id_recv,2);
			memcpy(&usart_send[19],&recnearbaselist_id,2*rec_nearbase_num);
			memcpy(&usart_send[19+rec_nearbase_num2],&recnearbaselist_dist,4rec_nearbase_num+4);

			checksum = Checksum_u16(&usart_send[2],23+8*rec_nearbase_num);
			memcpy(&usart_send[25+8*rec_nearbase_num],&checksum,2);
			UART_PushFrame(usart_send,27+8*rec_nearbase_num);

			//////////////////////////////////////////////////////////////


			GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
			if(anc_id_recv==dev_id)
			{
			Anchor_RecNearPoll(0);
			Anchor_RecNearPoll(rec_nearbase_num);
			}else{

			for(i=0;i<rec_nearbase_num;i++)
			@@ -1188,8 +1361,9 @@
			memcpy(&tempid,&rx_buffer[NEARBASEID_INDEX+i*2],2);
			if(tempid==dev_id)
			{

			seize_anchor=0; //éæ¢å ãå·²åå¨åè¡¨ä¸
			Anchor_RecNearPoll(i+1);
			Anchor_RecNearPoll(i);
			break;
			}
			}
			@@ -1200,9 +1374,13 @@
			// seize_anchor=1; //æ¢å anchor
			// Anchor_RecNearPoll(i);
			// }

			// if(tag_id_recv==0x4008)
			// printf("æ ç¾é«åº¦: %d,åºç«é«åº¦: %d,é«åº¦å·®ï¼ %d. \r\n",rec_tagheight,intheight,intheight-rec_tagheight);

			break;
			break;
			default:

			break;
			}
			}else{
			#ifdef DEBUG_INF