UWB_SmallModule_F103.git

			@@ -1,93 +1,12 @@

			/*! ----------------------------------------------------------------------------
			* @file main.c
			* @brief Double-sided two-way ranging (DS TWR) initiator example code
			*
			*
			*
			* @attention
			*
			* Copyright 2015 (c) Decawave Ltd, Dublin, Ireland.
			*
			* All rights reserved.
			*
			* @author Decawave
			*/

			#include <string.h>
			#include "dw_app.h"
			#include "deca_device_api.h"
			#include "deca_regs.h"
			#include "dw_driver.h"
			#include "Spi.h"
			#include "led.h"
			#include "serial_at_cmd_app.h"
			#include "Usart.h"
			#include "global_param.h"
			#include "filters.h"
			#include <stdio.h>
			#include "beep.h"


			/------------------------------------ Marcos ------------------------------------------/
			/* Inter-ranging delay period, in milliseconds. */
			#define RNG_DELAY_MS 100

			/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
			#define TX_ANT_DLY 0
			#define RX_ANT_DLY 32899

			/* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
			* 1 uus = 512 / 499.2 ç¥ and 1 ç¥ = 499.2 * 128 dtu. */
			#define UUS_TO_DWT_TIME 65536

			/* Delay between frames, in UWB microseconds. See NOTE 4 below. */
			/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
			#define POLL_TX_TO_RESP_RX_DLY_UUS 150
			/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the
			* frame length of approximately 2.66 ms with above configuration. */
			#define RESP_RX_TO_FINAL_TX_DLY_UUS 400
			/* Receive response timeout. See NOTE 5 below. */
			#define RESP_RX_TIMEOUT_UUS 600

			#define POLL_RX_TO_RESP_TX_DLY_UUS 420
			/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
			#define RESP_TX_TO_FINAL_RX_DLY_UUS 200
			/* Receive final timeout. See NOTE 5 below. */
			#define FINAL_RX_TIMEOUT_UUS 4300

			#define SPEED_OF_LIGHT 299702547

			/* Indexes to access some of the fields in the frames defined above. */
			#define FINAL_MSG_POLL_TX_TS_IDX 10
			#define FINAL_MSG_RESP_RX_TS_IDX 14
			#define FINAL_MSG_FINAL_TX_TS_IDX 18
			#define FINAL_MSG_TS_LEN 4

			#define SYNC_SEQ_IDX 5

			#define GROUP_ID_IDX 0
			#define ANCHOR_ID_IDX 1
			#define TAG_ID_IDX 5
			#define MESSAGE_TYPE_IDX 9
			#define DIST_IDX 10
			//Poll
			#define ANC_TYPE_IDX 14
			#define BATTARY_IDX 15
			#define BUTTON_IDX 16
			#define SEQUENCE_IDX 17
			//respose
			#define ANCTIMEMS 14
			#define ANCTIMEUS 16
			#define TAGSLOTPOS 18

			#define POLL 0x01
			#define RESPONSE 0x02
			#define FINAL 0x03
			#define SYNC 0x04

			/------------------------------------ Variables ------------------------------------------/
			/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
			#include "ADC.h"
			#define TDFILTER
			enum enumtagstate
			{
			DISCPOLL,
			GETNEARMSG,
			NEARPOLL,
			}tag_state=NEARPOLL;
			static dwt_config_t config = {
			2, /* Channel number. */
			DWT_PRF_64M, /* Pulse repetition frequency. */
			@@ -100,67 +19,38 @@
			DWT_PHRMODE_STD, /* PHY header mode. */
			(129 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
			};

			/* Frames used in the ranging process. See NOTE 2 below. */
			static uint8_t tx_poll_msg[20] = {0};
			static uint8_t tx_sync_msg[14] = {0};
			//static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
			static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

			//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
			static uint8_t tx_final_msg[60] = {0};
			static uint8_t tx_resp_msg[22] = {0};
			//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};

			/* Frame sequence number, incremented after each transmission. */
			static uint32_t frame_seq_nb = 0;

			/* Hold copy of status register state here for reference, so reader can examine it at a breakpoint. */
			static uint8_t tx_nearpoll_msg[80] = {0};
			static uint8_t tx_nearresp_msg[80] = {0};
			static uint8_t tx_nearfinal_msg[80] = {0};

			static uint8_t tx_near_msg[80] = {0};

			uint32_t frame_seq_nb = 0, frame_seq_nb2 = 0;
			static uint32_t status_reg = 0;

			/* Buffer to store received response message.
			* Its size is adjusted to longest frame that this example code is supposed to handle. */
			#define RX_BUF_LEN 24
			static uint8_t rx_buffer[RX_BUF_LEN];

			/* Time-stamps of frames transmission/reception, expressed in device time units.
			* As they are 40-bit wide, we need to define a 64-bit int type to handle them. */
			static uint8_t rx_buffer[100];
			static uint64_t poll_tx_ts;
			static uint64_t resp_rx_ts;
			static uint64_t final_tx_ts;

			/* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */
			static uint64_t poll_rx_ts;
			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]; ;
			uint32_t tag_id = 0;
			uint32_t tag_id_recv = 0;
			uint32_t anc_id_recv = 0;
			uint8_t random_delay_tim = 0;

			double distance, dist_no_bias, dist_cm;

			uint32_t g_UWB_com_interval = 0;
			float dis_after_filter; //å½åè·ç¦»å¼
			LPFilter_Frac* p_Dis_Filter; //æµè·ç¨çä½éæ»¤æ³¢å¨

			uint16_t g_Tagdist[TAG_NUM_IN_SYS];
			uint8_t g_flag_Taggetdist[256];
			/------------------------------------ Functions ------------------------------------------/


			/*! ------------------------------------------------------------------------------------------------------------------
			* @fn get_tx_timestamp_u64()
			*
			* @brief Get the TX time-stamp in a 64-bit variable.
			* /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX!
			*
			* @param none
			*
			* @return 64-bit value of the read time-stamp.
			*/
			static uint64_t get_tx_timestamp_u64(void)
			{
			uint8_t ts_tab[5];
			@@ -175,16 +65,6 @@
			return ts;
			}

			/*! ------------------------------------------------------------------------------------------------------------------
			* @fn get_rx_timestamp_u64()
			*
			* @brief Get the RX time-stamp in a 64-bit variable.
			* /!\ This function assumes that length of time-stamps is 40 bits, for both TX and RX!
			*
			* @param none
			*
			* @return 64-bit value of the read time-stamp.
			*/
			static uint64_t get_rx_timestamp_u64(void)
			{
			uint8_t ts_tab[5];
			@@ -199,17 +79,6 @@
			return ts;
			}

			/*! ------------------------------------------------------------------------------------------------------------------
			* @fn final_msg_set_ts()
			*
			* @brief Fill a given timestamp field in the final message with the given value. In the timestamp fields of the final
			* message, the least significant byte is at the lower address.
			*
			* @param ts_field pointer on the first byte of the timestamp field to fill
			* ts timestamp value
			*
			* @return none
			*/
			static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
			{
			int i;
			@@ -276,10 +145,13 @@
			tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
			tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;

			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);
			memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 4);
			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);
			memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 2);
			memcpy(&tx_nearresp_msg[ANCHOR_ID_IDX], &dev_id, 2);
			memcpy(&tx_nearpoll_msg[TAG_ID_IDX], &dev_id, 2);
			memcpy(&tx_nearfinal_msg[TAG_ID_IDX], &dev_id, 2);
			}
			uint16_t Checksum_u16(uint8_t* pdata, uint32_t len)
			{
			@@ -292,7 +164,7 @@
			}

			u16 tag_time_recv[TAG_NUM_IN_SYS];
			u8 usart_send[25];
			u8 usart_send[100],usart_send_anc[100];
			u8 battary,button;
			extern uint8_t g_pairstart;
			void tag_sleep_configuraion(void)
			@@ -302,14 +174,23 @@
			}
			extern uint8_t g_start_send_flag;
			u8 g_start_sync_flag;
			void SyncPoll(u8 sync_seq)
			{
			g_start_sync_flag=1;
			dwt_forcetrxoff();
			void SyncPoll(u8 sync_seq,uint32_t sync_baseid)
			{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);
			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);
			if(result==0)
			{
			while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//ä¸ææ¥è¯¢è¯çç¶æç´å°åéå®æ
			{ };
			}
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//æ¸æ¥æ å¿ä½
			}
			uint16_t g_Resttimer;
			uint8_t result;
			@@ -321,174 +202,530 @@
			u16 tmp_time;
			int32_t temp_dist;
			u16 tagslotpos;
			void Tag_App(void)//åéæ¨¡å¼(TAGæ ç¾)

			u16 anclist_num=0,anclist_pos; //list æ»æ°éåå½åä½ç½®
			u16 ancid_list[TAG_NUM_IN_SYS];
			u8 nearbase_num=0,last_nearbase_num,next_nearbase_num;
			u16 nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM];
			int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM];
			uint8_t trygetnearmsg_times;
			u16 current_slotnum,total_slotnum,target_time,last_time;
			u32 rec_tagpos_binary;
			extern uint32_t tagpos_binary;
			void SetNextPollTime(u16 time)
			{
			current_slotnum++;
			if(current_slotnum==total_slotnum)
			current_slotnum-=total_slotnum;
			//time=5;
			target_time=((current_slotnum*g_com_map[COM_INTERVAL])+time);

			last_time=target_time;
			if(target_time>=990&&target_time<992)
			{
			current_slotnum++;
			target_time+=g_com_map[COM_INTERVAL];
			tagslotpos=GetRandomSlotPos(rec_tagpos_binary\|tagpos_binary);
			tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
			}
			if(target_time>=1000)
			{target_time-=1000;}
			if(target_time<0)
			{target_time+=1000;}

			}
			u8 FindNearBasePos(u16 baseid)
			{
			u8 i;
			for(i=0;i<last_nearbase_num;i++)
			{
			if(baseid==nearbaseid_list[i])
			return i;
			}
			return i;
			}
			u8 recbase_num=0;
			#define CHANGE_BASE_THRESHOLD 5
			//void NearAncSelect(void)
			//{static u16 last_mainbase_id,change_base_count;
			// int32_t nearbase_mindist=99999, nearbase_minpos;
			// u8 i;
			// for(i=0;i<recbase_num-1;i++)
			// {
			// if(nearbase_mindist>nearbase_distlist[i])
			// {
			// nearbase_mindist=nearbase_distlist[i];
			// nearbase_minpos=i;
			// }
			// }
			// if(nearbase_mindist<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST)
			// {
			// if(last_mainbase_id==nearbaseid_list[nearbase_minpos])
			// {
			// change_base_count++;
			// if(change_base_count>CHANGE_BASE_THRESHOLD)
			// {
			// mainbase_id=last_mainbase_id;
			// tag_state=GETNEARMSG;
			// }
			// }else{
			// change_base_count=0;
			// }
			// last_mainbase_id=nearbaseid_list[nearbase_minpos];
			// }else{
			// change_base_count=0;
			// }
			//}
			uint8_t GetRandomValue(void)
			{
			uint8_t random_value=0,temp_adc,i;
			for(i=0;i<8;i++)
			{
			temp_adc=Get_ADC_Value();
			random_value=random_value\|((temp_adc&0x01)<<i);
			}
			return random_value;
			}
			u8 GetRandomSlotPos(uint32_t emptyslot)
			{
			u8 i,temp_value;
			temp_value = GetRandomValue();
			for(i=temp_value%max_slotpos;i<max_slotpos;i++)
			{
			if(((emptyslot>>i)&0x1)==0)
			{
			return i;
			}
			}

			for(i=1;i<max_slotpos;i++)
			{
			if(((emptyslot>>i)&0x1)==0)
			{
			return i;
			}
			}
			return max_slotpos-1;
			}
			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;

			void NearPoll(void)
			{
			static u8 mainbase_lost_count=0,flag_finalsend,flag_rxon;
			uint32_t temp1,temp2,dw_systime;
			uint32_t frame_len;
			uint32_t final_tx_time;
			u32 start_poll;
			u8 i,getsync_flag=0;
			//LED0_ON;
			dwt_forcetrxoff();
			g_Resttimer=0;
			u8 i,j,getsync_flag=0,timeout;
			// printf("%d",sync_timer);
			dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //è®¾ç½®åéåå¼å¯æ¥æ¶ï¼å¹¶è®¾å®å»¶è¿æ¶é´
			dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
			tag_succ_times = 0;
			tx_poll_msg[BATTARY_IDX] = Get_Battary();
			tx_poll_msg[BUTTON_IDX] = !READ_KEY0;
			tx_poll_msg[SEQUENCE_IDX] = frame_seq_nb++;
			GPIO_WriteBit(GPIOA, GPIO_Pin_9, Bit_RESET);
			for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			if(next_nearbase_num>=MAX_NEARBASE_NUM)
			{
			/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
			tx_poll_msg[ANC_TYPE_IDX] = i;

			dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(sizeof(tx_poll_msg), 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦

			/* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
			* set by dwt_setrxaftertxdelay() has elapsed. */
			next_nearbase_num = MAX_NEARBASE_NUM-1;
			}
			//nearbase_num=0;
			last_nearbase_num=next_nearbase_num;
			nearbase_num=next_nearbase_num;
			recbase_num=0;
			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);
			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_starttx(DWT_START_TX_IMMEDIATE \| DWT_RESPONSE_EXPECTED);//å¼å¯åéï¼åéå®æåçå¾ä¸æ®µæ¶é´å¼å¯æ¥æ¶ï¼çå¾æ¶é´å¨dwt_setrxaftertxdelayä¸è®¾ç½®

			flag_finalsend=0;
			flag_rxon=1;
			neartimout_timer=0;
			get_newbase=0;
			timeout=ceil((float)nearbase_num*SLOT_SCALE)+3;
			//timeout=5;
			mainbase_dist=100000;
			mainbase_lost_count++;
			while(neartimout_timer<timeout)
			{
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))//ä¸ææ¥è¯¢è¯çç¶æç´å°æåæ¥æ¶æèåçéè¯¯
			{
			if(flag_finalsend)
			{
			dw_systime=dwt_readsystimestamphi32();
			if(dw_systime>temp1&&dw_systime<temp2)
			{
			flag_finalsend=0;
			flag_rxon=0;
			dwt_forcetrxoff();
			dwt_setdelayedtrxtime(final_tx_time);
			result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé
			break;
			}
			}
			if(neartimout_timer>timeout)
			break;
			};
			if(status_reg==0xffffffff)
			{
			NVIC_SystemReset();
			}
			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); //è¯»åæ¥æ¶æ°æ®
			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æ°æ®
			{ u16 rec_nearbaseid,rec_nearbasepos;
			int32_t temp_dist;
			poll_tx_ts = get_tx_timestamp_u64(); //è·å¾POLLåéæ¶é´T1
			resp_rx_ts = get_rx_timestamp_u64(); //è·å¾RESPONSEæ¥æ¶æ¶é´T4
			recbase_num++;
			memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
			if(last_nearbase_num==0)
			{
			get_newbase=1;
			nearbaseid_list[0]=rec_nearbaseid;
			nearbase_num=1;
			memcpy(&tx_nearfinal_msg[ANCHOR_ID_IDX],&rec_nearbaseid,2);
			}
			if(rec_nearbaseid==nearbaseid_list[0])
			{
			//////////////////////////////////æ¶é´åæ¥
			temp_sync_timer2=sync_timer;
			memcpy(&temp_sync_timer1,&rx_buffer[ANCTIMEMS],2);
			memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);


			if(g_com_map[DEV_ROLE]!=0)
			{
			sync_timer=temp_sync_timer1;
			//// memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);
			//

			tmp_time=tmp_time+650;
			if(tmp_time>999)
			{
			tmp_time-=999;
			sync_timer++;
			if(sync_timer>=1000)
			{sync_timer=0;}
			}
			TIM3->CNT=tmp_time;
			}
			current_slotnum=temp_sync_timer1/g_com_map[COM_INTERVAL];
			// if(tagslotpos>max_slotpos)
			// tagslotpos=tagslotpos%(max_slotpos+1);
			// tyncpoll_time=(tagslotpos-1)*slottime;
			////////////////////////////
			rec_nearbasepos=0;

			memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
			nearbase_distlist[rec_nearbasepos]=temp_dist;
			if(temp_dist!=0x1ffff)
			exsistbase_list[rec_nearbasepos]=KEEP_TIMES;

			mainbase_lost_count=0;
			flag_finalsend=1;
			final_tx_time = (resp_rx_ts + ((RESP_RX_TO_FINAL_TX_DLY_UUS+DELAY_BETWEEN_TWO_FRAME_UUSnearbase_num+500) UUS_TO_DWT_TIME)) >> 8;
			temp1=final_tx_time-((350*UUS_TO_DWT_TIME)>>8);
			temp2=final_tx_time+((100*UUS_TO_DWT_TIME)>>8);
			// dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//finalåå®éåéæ¶é´æ¯è®¡ç®æ¶é´å ä¸åéå¤©çº¿delay
			final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//å°T1ï¼T4ï¼T5åå¥åéæ°æ®
			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;
			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_Resttimer=0;
			#ifndef USART_INTEGRATE_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;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			#endif

			//dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			// result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé
			//dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4);//å°åéæ°æ®åå¥DW1000
			}


			}else{
			rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
			if(rec_nearbasepos>=last_nearbase_num) //åç°æ°çåºç«
			{
			get_newbase=1;
			nearbase_num++;
			nearbaseid_list[rec_nearbasepos] = rec_nearbaseid;
			memcpy(&tx_nearfinal_msg[ANCHOR_ID_IDX],&rec_nearbaseid,2);
			}

			memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
			nearbase_distlist[rec_nearbasepos]=temp_dist;

			if(temp_dist!=0x1ffff)
			exsistbase_list[rec_nearbasepos]=KEEP_TIMES;

			final_msg_set_ts(&tx_nearfinal_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
			dwt_writetxdata(28+nearbase_num*4, tx_nearfinal_msg, 0);//å°åéæ°æ®åå¥DW1000
			dwt_writetxfctrl(28+nearbase_num*4, 0);//è®¾å®åéæ°æ®é¿åº¦
			LED0_BLINK;
			// memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
			// tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
			if(temp_dist!=0x1ffff)
			{
			#ifndef USART_INTEGRATE_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;
			checksum = Checksum_u16(&usart_send[2],17);
			memcpy(&usart_send[19],&checksum,2);
			UART_PushFrame(usart_send,21);
			#endif

			//dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5
			// result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé
			//dwt_writetxdata(4,&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4], FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)4);//å°åéæ°æ®åå¥DW1000
			}
			}
			}
			}else{
			dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG\| SYS_STATUS_ALL_RX_ERR);
			if(flag_rxon)
			{dwt_rxenable(0);
			}
			}
			}
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			// if(result==0)
			// {
			// while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//ä¸ææ¥è¯¢è¯çç¶æç´å°åéå®æ
			// { };
			// }
			// dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//æ¸æ¥æ å¿ä½

			dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG\| SYS_STATUS_ALL_RX_ERR);
			nearbase_num=recbase_num;
			j=0;
			if(exsistbase_list[0]==0)
			{
			// u8 temp_adc,random_value;
			// random_value=0;
			// for(i=0;i<8;i++)
			// {
			// temp_adc=Get_ADC_Value();
			// random_value=random_value\|((temp_adc&0x01)<<i);
			// }
			tagslotpos=GetRandomSlotPos(rec_tagpos_binary\|tagpos_binary);
			tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
			tag_state=NEARPOLL;
			}
			// tyncpoll_time=0;
			next_nearbase_num=0;
			for(i=0;i<last_nearbase_num+get_newbase;i++)
			{
			if(exsistbase_list[i]>0)
			{
			next_nearbase_num++;
			true_exsistbase_list[j]=exsistbase_list[i];
			true_nearbase_idlist[j]=nearbaseid_list[i];
			true_nearbase_distlist[j++]=nearbase_distlist[i];
			exsistbase_list[i]--;
			}
			}

			if(recbase_num<3)
			{next_nearbase_num=next_nearbase_num;}
			last_nearbase_num = next_nearbase_num;
			for(i=0;i<last_nearbase_num-1;i++)
			{
			for(j=0;j<last_nearbase_num-1;j++)
			{
			if(true_nearbase_distlist[j]>true_nearbase_distlist[j+1])
			{
			u32 temp_dist,temp_id,temp_exsis;
			temp_dist=true_nearbase_distlist[j];
			temp_id = true_nearbase_idlist[j];
			temp_exsis=true_exsistbase_list[i];
			true_nearbase_distlist[j]=true_nearbase_distlist[j+1];
			true_nearbase_idlist[j]=true_nearbase_idlist[j+1];
			true_exsistbase_list[j]=true_exsistbase_list[j+1];

			true_nearbase_distlist[j+1]=temp_dist;
			true_nearbase_idlist[j+1]=temp_id;
			true_exsistbase_list[j+1]=temp_exsis;
			}
			}
			}

			report_num=0;
			for (i=0;i<last_nearbase_num;i++)
			{
			nearbaseid_list[i]=true_nearbase_idlist[i];
			nearbase_distlist[i]=true_nearbase_distlist[i];
			if(nearbase_distlist[i]!=0x1ffff&&true_exsistbase_list[i]==KEEP_TIMES)
			{
			memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2);
			memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4);
			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
			usart_send[2] = 4;//æ£å¸¸æ¨¡å¼
			usart_send[3] = report_num*6+2;//æ£å¸¸æ¨¡å¼
			checksum = Checksum_u16(&usart_send[2],report_num*6+2);
			memcpy(&usart_send[4+report_num*6],&checksum,2);
			UART_PushFrame(usart_send,6+report_num*6);
			#endif
			// if(mainbase_lost_count>5)
			// {
			// //tag_state=DISCPOLL;
			// }
			// NearAncSelect();
			// if(recbase_num<g_com_map[MIN_REPORT_ANC_NUM] )
			// {
			// anclost_times++;
			// if(anclost_times>3)
			// {
			// tagslotpos=poll_tx_ts%(max_slotpos+1);
			// }
			// }else{
			// anclost_times=0;
			// }
			dwt_forcetrxoff();
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			}
			u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32];
			void GetNearMsg(void)
			{
			u32 start_poll,frame_len;
			if(nearmsg_mainbase)
			{nearmsg_mainbase=0;
			}else{
			nearmsg_mainbase=1;
			}
			memcpy(&tx_near_msg[ANCHOR_ID_IDX],&nearmsg_mainbase,2);
			memcpy(&tx_near_msg[TAG_ID_IDX],&dev_id,2);
			tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG;

			dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS); //è®¾ç½®åéåå¼å¯æ¥æ¶ï¼å¹¶è®¾å®å»¶è¿æ¶é´
			dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
			dwt_writetxdata(12, tx_near_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(12, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE\| DWT_RESPONSE_EXPECTED);
			start_poll = time32_incr;
			/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR)))//ä¸ææ¥è¯¢è¯çç¶æç´å°æåæ¥æ¶æèåçéè¯¯
			{ if(time32_incr - start_poll>20)
			NVIC_SystemReset();
			UART_CheckReceive();

			};

			/* Increment frame sequence number after transmission of the poll message (modulo 256). */
			if(status_reg==0xffffffff)
			{
			NVIC_SystemReset();
			}

			if (status_reg & SYS_STATUS_RXFCG)//å¦ææåæ¥æ¶
			{
			/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG \| SYS_STATUS_TXFRS);//æ¸æ¥å¯åå¨æ å¿ä½

			/* A frame has been received, read it into the local buffer. */
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK; //è·å¾æ¥æ¶å°çæ°æ®é¿åº¦

			dwt_readrxdata(rx_buffer, frame_len, 0); //è¯»åæ¥æ¶æ°æ®


			/* 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[MESSAGE_TYPE_IDX] == NEAR_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //å¤ææ¥æ¶å°çæ°æ®æ¯å¦æ¯responseæ°æ®
			{
			/* Retrieve poll transmission and response reception timestamp. */
			poll_tx_ts = get_tx_timestamp_u64(); //è·å¾POLLåéæ¶é´T1
			resp_rx_ts = get_rx_timestamp_u64(); //è·å¾RESPONSEæ¥æ¶æ¶é´T4

			if(getsync_flag==0)
			{
			getsync_flag=1;
			memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
			memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
			memcpy(&tagslotpos,&rx_buffer[TAGSLOTPOS],2);
			tmp_time=tmp_time+450;
			if(tmp_time>999)
			{
			tmp_time-=999;
			sync_timer++;
			if(sync_timer>=1010)
			{sync_timer=0;}
			}
			TIM3->CNT=tmp_time;
			if(tagslotpos>max_slotpos)
			tagslotpos=tagslotpos%(max_slotpos+1);
			tyncpoll_time=(tagslotpos-1)*slottime;
			next_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
			memcpy(&rec_tagpos_binary,&rx_buffer[NEARMSG_EMPTYSLOTPOS_INDEX],4);
			tagslotpos=GetRandomSlotPos(rec_tagpos_binary);
			//tagslotpos=rx_buffer[TAGSLOTPOS];
			memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],next_nearbase_num*2);
			//slottime=ceil((nearbase_num+2)*0.4)+2;
			tyncpoll_time=tagslotpos*slottime;
			tag_state=NEARPOLL;
			}else{
			tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
			}
			memcpy(&anchor_dist_last_frm[0], &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
			dwt_setdelayedtrxtime(final_tx_time);//è®¾ç½®finalååéæ¶é´T5

			/* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
			final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//finalåå®éåéæ¶é´æ¯è®¡ç®æ¶é´å ä¸åéå¤©çº¿delay

			/* Write all timestamps in the final message. See NOTE 10 below. */
			final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//å°T1ï¼T4ï¼T5åå¥åéæ°æ®
			final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
			final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);

			/* Write and send final message. See NOTE 7 below. */

			dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//å°åéæ°æ®åå¥DW1000
			dwt_writetxfctrl(sizeof(tx_final_msg), 0);//è®¾å®åéæ°æ®é¿åº¦
			result=dwt_starttx(DWT_START_TX_DELAYED);//è®¾å®ä¸ºå»¶è¿åé

			tag_succ_times++;

			LED0_BLINK;
			// temp_dist=anchor_dist_last_frm[0];
			// if(anchor_dist_last_frm[0]>1000000)
			// {
			// temp_dist=anchor_dist_last_frm[0]-0xffffffff;
			// temp_dist+=0xffff;
			// anchor_dist_last_frm[0]=temp_dist;
			// }


			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);
			hex_dist = anchor_dist_last_frm[0]+(int16_t)g_com_map[DIST_OFFSET];
			memcpy(&usart_send[9],&hex_dist,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);

			/* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
			if(result==0)
			{while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//ä¸ææ¥è¯¢è¯çç¶æç´å°åéå®æ
			{ };
			}
			/* Clear TXFRS event. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//æ¸é¤æ å¿ä½

			/* Increment frame sequence number after transmission of the final message (modulo 256). */

			random_delay_tim = 0;
			}
			else
			{
			random_delay_tim = DFT_RAND_DLY_TIM_MS; //å¦æéè®¯å¤±è´¥ï¼å°é´éæ¶é´å¢å 5msï¼é¿å¼å ä¸ºå¤æ ç¾åæ¶åéå¼èµ·çå²çªã
			}
			}else{
			tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
			}
			else
			{
			/* Clear RX error events in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			random_delay_tim = DFT_RAND_DLY_TIM_MS;
			}
			}

			if(getsync_flag==0)
			{
			tagslotpos--;
			if(tagslotpos==0\|\|tagslotpos>max_slotpos)
			{
			tagslotpos=max_slotpos;
			}
			tyncpoll_time=(tagslotpos-1)*slottime;
			}

			#ifdef USART_INTEGRATE_OUTPUT
			usart_send[2] = 4;//æ£å¸¸æ¨¡å¼
			usart_send[3] = report_num*6+2;//æ£å¸¸æ¨¡å¼
			checksum = Checksum_u16(&usart_send[2],report_num*6+2);
			memcpy(&usart_send[4+report_num*6],&checksum,2);
			UART_PushFrame(usart_send,6+report_num*6);
			#endif
			}

			int8_t correction_time;
			void Tag_App(void)//åéæ¨¡å¼(TAGæ ç¾)
			{
			//LED0_ON;
			dwt_forcetrxoff();
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			switch(tag_state)
			{
			case DISCPOLL:

			break;
			case GETNEARMSG:
			GetNearMsg();
			break;
			case NEARPOLL:
			NearPoll();
			break;
			}

			SetNextPollTime(tyncpoll_time);
			g_start_send_flag = 0;
			GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			}

			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];
			u8 tagofflinetime[TAG_NUM_IN_SYS];
			int32_t tagdist_list[TAG_NUM_IN_SYS];
			void TagListUpdate(void)
			{
			u16 i,j=0,temp[TAG_NUM_IN_SYS];
			@@ -507,126 +744,58 @@
			for(i=0;i<taglist_num;i++)
			{
			if(memcmp(&tagid,&tagid_list[i],2)==0)
			return i+1;
			break;
			}
			return 0;
			return i;
			}

			void Anchor_App(void)
			uint32_t frame_len;
			uint32_t resp_tx_time;
			uint8_t rec_nearbase_num,anc_report_num;
			void Anchor_RecPoll(void)
			{
			uint32_t frame_len;
			uint32_t resp_tx_time;

			/* Clear reception timeout to start next ranging process. */
			dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´

			/* Activate reception immediately. */
			dwt_rxenable(0);//æå¼æ¥æ¶

			/* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{
			IdleTask();
			g_Resttimer=0;
			};

			if (status_reg & SYS_STATUS_RXFCG)//æåæ¥æ¶
			{ u16 tag_recv_interval;
			/* Clear good RX frame event in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//æ¸é¤æ å¿ä½

			/* A frame has been received, read it into the local buffer. */
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//è·å¾æ¥æ¶æ°æ®é¿åº¦

			dwt_readrxdata(rx_buffer, frame_len, 0);//è¯»åæ¥æ¶æ°æ®


			/* Check that the frame is a poll sent by "DS TWR initiator" example.
			* As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */


			//å°æ¶å°çtag_idåå«åå¥åæ¬¡éè®¯çåä¸ï¼ä¸ºå¤æ ç¾éè®¯æå¡ï¼é²æ¢ä¸æ¬¡éè®¯ä¸æ¥æ¶å°ä¸åIDæ ç¾çæ°æ®
			//tag_id_recv = rx_buffer[TAG_ID_IDX];
			memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],4);
			memcpy(&tx_resp_msg[TAG_ID_IDX],&tag_id_recv,4);
			taglist_pos=CmpTagInList(tag_id_recv);
			if(taglist_pos==0)
			{
			tagid_list[taglist_num++]=tag_id_recv;
			taglist_pos=taglist_num;
			}
			tagofflinetime[taglist_pos-1]=0;
			if (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);
			memcpy(&tx_resp_msg[ANCTIMEUS],&tmp_time,2);
			memcpy(&tx_resp_msg[TAGSLOTPOS],&taglist_pos,2);
			// if(correction_time>10)
			// {correction_time++;}

			/* Retrieve poll reception timestamp. */
			memcpy(&tx_resp_msg[TAGSLOTPOS],&taglist_pos,1);

			poll_rx_ts = get_rx_timestamp_u64();//è·å¾Pollåæ¥æ¶æ¶é´T2

			/* Set send time for response. See NOTE 8 below. */
			resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//è®¡ç®Responseåéæ¶é´T3ã
			dwt_setdelayedtrxtime(resp_tx_time);//è®¾ç½®Responseåéæ¶é´T3

			/* Set expected delay and timeout for final message reception. */
			dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);//è®¾ç½®åéå®æåå¼å¯æ¥æ¶å»¶è¿æ¶é´
			dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//æ¥æ¶è¶æ¶æ¶é´

			/* 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], &tagdist_list[taglist_pos], 4);
			memcpy(&tx_resp_msg[TAG_ID_IDX],&tag_id_recv,2);

			dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//åå¥åéæ°æ®
			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_nb = rx_buffer[SEQUENCE_IDX];
			/* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
			* See NOTE 7 below. */
			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)))///ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{ };
			}
			/* Increment frame sequence number after transmission of the response message (modulo 256). */

			if (status_reg & SYS_STATUS_RXFCG)//æ¥æ¶æå
			{
			/* Clear good RX frame event and TX frame sent in the DW1000 status register. */
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG \| SYS_STATUS_TXFRS);//æ¸æ¥æ å¿ä½

			/* A frame has been received, read it into the local buffer. */
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;//æ°æ®é¿åº¦

			dwt_readrxdata(rx_buffer, frame_len, 0);//è¯»åæ¥æ¶æ°æ®


			/* 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[MESSAGE_TYPE_IDX] == 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;

			/* Retrieve response transmission and final reception timestamps. */
			resp_tx_ts = get_tx_timestamp_u64();//è·å¾responseåéæ¶é´T3
			final_rx_ts = get_rx_timestamp_u64();//è·å¾finalæ¥æ¶æ¶é´T6

			/* Get timestamps embedded in the final message. */
			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_TS_IDX], &resp_rx_ts);
			final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

			/* 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;
			final_rx_ts_32 = (uint32_t)final_rx_ts;
			@@ -635,60 +804,296 @@
			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[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½éæ»¤æ³¢å¨ï¼è®©æ°æ®æ´ç¨³å®

			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];
			if(tag_id_recv-TAG_ID_START<=TAG_NUM_IN_SYS)
			if(abs(hex_dist-his_dist[tag_id_recv-TAG_ID_START])<1000)
			{
			tagdist_list[taglist_pos] = hex_dist;
			}
			his_dist[taglist_pos]=hex_dist;
			}
			}else{
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}
			u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,getrange_success=0;
			u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase 1 first near_base
			{
			u8 motorstate;
			tmp_time=TIM3->CNT;
			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ã
			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;
			}

			if(new_tagid)
			{
			tagdist_list[taglist_pos]=0x1ffff;
			memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
			}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)
			{
			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)
			{
			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 * 1000; //dis ä¸ºåä½ä¸ºcmçè·ç¦»
			/--------------------------ä»¥ä¸ä¸ºéæµè·é»è¾------------------------/
			//dist_cm=33000;
			getrange_success = 1;
			g_Resttimer=0;
			LED0_BLINK; //æ¯æåä¸æ¬¡éè®¯åéªçä¸æ¬¡
			dis_after_filter=dist_cm;
			hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;
			g_flag_Taggetdist[taglist_pos]=0;
			if(hex_dist>-10000&&hex_dist<2000000)
			{
			g_Tagdist[tag_id_recv-TAG_ID_START] = hex_dist;
			anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = hex_dist;
			}
			his_dist[tag_id_recv-TAG_ID_START]=hex_dist;

			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			if(abs(hex_dist-his_dist[taglist_pos])<15000\|\|misdist_num[taglist_pos]>3)
			{
			int32_t filter_dist;
			#ifdef TDFILTER
			NewTrackingDiffUpdate(taglist_pos, (float)hex_dist);
			filter_dist=pos_predict[taglist_pos]/10;
			#else
			filter_dist=hex_dist/10;
			#endif
			misdist_num[taglist_pos]=0;
			tagdist_list[taglist_pos] = filter_dist;
			his_dist[taglist_pos]=hex_dist;
			g_Tagdist[taglist_pos]=filter_dist;
			#ifndef USART_INTEGRATE_OUTPUT
			usart_send[2] = 1;//æ£å¸¸æ¨¡å¼
			usart_send[3] = 17;//æ°æ®æ®µé¿åº¦
			usart_send[4] = frame_seq_nb;//æ°æ®æ®µé¿åº¦
			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],&anchor_dist_last_frm[tag_id_recv-TAG_ID_START],4);
			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
			}else{
			// printf("%d",hex_dist);
			misdist_num[taglist_pos]++;
			}
			}
			}
			}else{
			/* Clear RX error events in the DW1000 status register. */
			//printf("%x/n",status_reg);
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}else if(rx_buffer[MESSAGE_TYPE_IDX] == SYNC)
			}
			uint32_t current_syncid=0xffffffff,synclost_timer;
			extern u8 flag_syncbase;
			u8 tagpos_rec[50],tagpos_send[50],ancidlist_num;
			u16 ancidlist_rec[20],ancidlist_send[20];
			void Anchor_App(void)
			{

			u8 send_len,i;
			u16 tempid;
			uint32_t rec_syncid;

			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR\| SYS_STATUS_TXFRS \|SYS_STATUS_RXFCG);
			g_start_sync_flag=0;

			dwt_setrxtimeout(0);//è®¾å®æ¥æ¶è¶æ¶æ¶é´ï¼0ä½æ²¡æè¶æ¶æ¶é´
			dwt_rxenable(0);//æå¼æ¥æ¶
			// GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
			while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG \| SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//ä¸ææ¥è¯¢è¯çç¶æç´å°æ¥æ¶æåæèåºç°éè¯¯
			{

			IdleTask();
			};
			//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
			if (status_reg & SYS_STATUS_RXFCG)//æåæ¥æ¶
			{ u16 tag_recv_interval;
			float temp_tagpos;


			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//æ¸é¤æ å¿ä½
			frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//è·å¾æ¥æ¶æ°æ®é¿åº¦
			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);
			switch(rx_buffer[MESSAGE_TYPE_IDX])
			{
			if(rx_buffer[SYNC_SEQ_IDX]<sync_seq)
			// if(rx_buffer[SYNC_SEQ_IDX]==2)
			{
			sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
			TIM3->CNT = sync_seq*325%1000+15;
			sync_timer = sync_seq*325/1000;
			SyncPoll(sync_seq);
			}
			}
			}
			else
			{
			/* Clear RX error events in the DW1000 status register. */
			// case POLL:
			// if (anchor_type == rx_buffer[ANC_TYPE_IDX])
			// Anchor_RecPoll();
			// break;
			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;
			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);
			}

			}
			break;
			case NEAR_MSG:
			if(anc_id_recv==flag_syncbase)
			{
			rx_buffer[TAGSLOTPOS]=taglist_pos;
			tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_MSG;
			memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
			memcpy(&tx_near_msg[NEARMSG_EMPTYSLOTPOS_INDEX],&tagpos_binary,4);
			tx_near_msg[NEARBASENUM_INDEX]=ancidlist_num;
			memcpy(&tx_near_msg[NEARBASEID_INDEX],&ancidlist_send,ancidlist_num*2);
			send_len=11+ancidlist_num*2+2;
			dwt_writetxdata(send_len, tx_near_msg, 0);//å°Pollåæ°æ®ä¼ ç»DW1000ï¼å°å¨å¼å¯åéæ¶ä¼ åºå»
			dwt_writetxfctrl(send_len, 0);//è®¾ç½®è¶å®½å¸¦åéæ°æ®é¿åº¦
			dwt_starttx(DWT_START_TX_IMMEDIATE);
			}
			break;

			case NEAR_POLL:
			memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
			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;
			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];

			if(rec_nearbase_num>ancidlist_num)
			{
			ancidlist_num=rec_nearbase_num;
			memcpy(ancidlist_rec,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
			}

			for(i=0;i<rec_nearbase_num;i++)
			{
			memcpy(&tempid,&rx_buffer[NEARBASEID_INDEX+i*2],2);
			if(tempid==dev_id)
			{
			seize_anchor=0; //éæ¢å ãå·²åå¨åè¡¨ä¸
			Anchor_RecNearPoll(i);
			break;
			}
			}
			if(i==rec_nearbase_num)
			{
			seize_anchor=1; //æ¢å anchor
			Anchor_RecNearPoll(i);
			}


			break;
			}
			}else{
			dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
			}