#include "dw_app.h"
#include "ADC.h"
#include "modbus.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. */
	DWT_PLEN_128,    /* 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. */
	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. */
};
static uint8_t tx_poll_msg[20] = {0};
static uint8_t tx_sync_msg[14] = {0};
static uint8_t tx_final_msg[60] = {0};
static uint8_t tx_resp_msg[22] = {0};
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;
static uint8_t rx_buffer[100];
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
static uint64_t final_tx_ts;
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;		//²â¾àÓõĵÍͨÂ˲¨Æ÷
int32_t g_Tagdist[TAG_NUM_IN_SYS];
uint8_t g_flag_Taggetdist[256];

static uint64_t get_tx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readtxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
    {
        ts <<= 8;
        ts |= ts_tab[i];
    }
    return ts;
}

static uint64_t get_rx_timestamp_u64(void)
{
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readrxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
    {
        ts <<= 8;
        ts |= ts_tab[i];
    }
    return ts;
}

static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
{
    int i;
    for (i = 0; i < FINAL_MSG_TS_LEN; i++)
    {
        ts_field[i] = (uint8_t) ts;
        ts >>= 8;
    }
}

static void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts)
{
    int i;
    *ts = 0;
    for (i = 0; i < FINAL_MSG_TS_LEN; i++)
    {
        *ts += ts_field[i] << (i * 8);
    }
}
void TagDistClear(void)
{
	static uint16_t clear_judge_cnt;
	uint16_t i;
	if(clear_judge_cnt++>1000)  //É趨1S·ÖƵ£¬Ã¿Ãë½øÒ»´Î¡£Åжϱê־λ´óÓÚµÈÓÚ2£¬2sûÊÕµ½Êý¾Ý¾Í°ÑÊý¾Ý±ä³É0xffff£¬²»´¥·¢¾¯±¨¡£
	{
		clear_judge_cnt=0;
		for(i=0;i<255;i++)
		{
			g_flag_Taggetdist[i]++;
			if(g_flag_Taggetdist[i]>=2)
			{
				g_Tagdist[i]=0x1ffff;
			}
		}
	}
}
extern int16_t g_commap_antdelay;
void Dw1000_Init(void)
{
	/* Reset and initialise DW1000.
     * For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum
     * performance. */
    Reset_DW1000();//ÖØÆôDW1000 /* Target specific drive of RSTn line into DW1000 low for a period. */
    dwt_initialise(DWT_LOADUCODE);//³õʼ»¯DW1000
	Spi_ChangePrescaler(SPIx_PRESCALER_FAST);	//ÉèÖÃΪ¿ìËÙģʽ

    /* Configure DW1000. See NOTE 6 below. */
    dwt_configure(&config);//ÅäÖÃDW1000
	

	
    /* Apply default antenna delay value. See NOTE 1 below. */
    dwt_setrxantennadelay(RX_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;
	tx_poll_msg[MESSAGE_TYPE_IDX]=POLL;
	tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE;
	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);
	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) 
{
    uint16_t sum = 0;
    uint32_t i;
    for(i=0; i<len; i++)
        sum += pdata[i];
    sum = ~sum;
    return sum;
}

u16 tag_time_recv[TAG_NUM_IN_SYS];
u8 usart_send[100],usart_send_anc[100];
u8 battary,button;
extern uint8_t g_pairstart;
void tag_sleep_configuraion(void)
{
	dwt_configuresleep(0x940, 0x7);
	dwt_entersleep();
}
extern uint8_t g_start_send_flag;
u8 g_start_sync_flag;
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;
u8 tag_succ_times=0;
int32_t hex_dist;
u16 checksum;
int8_t tag_delaytime;
extern uint16_t sync_timer;
u16 tmp_time;
int32_t temp_dist;
u16 tagslotpos;

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,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;
	GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
	if(next_nearbase_num>=MAX_NEARBASE_NUM)
	{
		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[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_num*2+i*2],&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(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;
	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)&&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
								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_UUS*nearbase_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;
                                    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)
									{
										g_Resttimer=0;
                                        
										#ifdef USART_SINGLE_OUTPUT
										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],&rec_nearbaseid,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)
									{
										#ifdef USART_SINGLE_OUTPUT
										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],&rec_nearbaseid,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++;
				}
			}

			
		//	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();
	};
	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);   //¶ÁÈ¡½ÓÊÕÊý¾Ý
		if (rx_buffer[MESSAGE_TYPE_IDX] == NEAR_MSG&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //ÅжϽÓÊÕµ½µÄÊý¾ÝÊÇ·ñÊÇresponseÊý¾Ý
		{
			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;
		}
	}else{
			tyncpoll_time = (tagslotpos--%max_slotpos)*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
}

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],tagstate_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];
	for(i=0;i<taglist_num;i++)
	{
		if(tagofflinetime[i]++<QUIT_SLOT_TIME)
		{
			tagid_list[j]=tagid_list[i];
			tagofflinetime[j++]=tagofflinetime[i];
		}
	}
	taglist_num=j;
}
u16 CmpTagInList(u16 tagid)
{u16 i;
	for(i=0;i<taglist_num;i++)
	{
		if(memcmp(&tagid,&tagid_list[i],2)==0)
			break;
	}
	return i;
}
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;
			memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
			memcpy(&tx_resp_msg[ANCTIMEUS],&tmp_time,2);
			memcpy(&tx_resp_msg[TAGSLOTPOS],&taglist_pos,1);
			
			poll_rx_ts = get_rx_timestamp_u64();//»ñµÃPoll°ü½ÓÊÕʱ¼äT2

			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
			dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);//ÉèÖ÷¢ËÍÍê³Éºó¿ªÆô½ÓÊÕÑÓ³Ùʱ¼ä
			dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//½ÓÊÕ³¬Ê±Ê±¼ä
			
			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_nb2 = rx_buffer[SEQUENCE_IDX];
			if(result==0)
			{
				while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
			{ };
		}
			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 (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;
					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_TS_IDX], &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];
					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);
			}
}
 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((F1*F1+F2*F2+F3*F3)/(N*N))-A;
    rx_power=10*log10(C*B/(N*N))-A;

 //   min_power =  - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
    return min_power;
  }
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;
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_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(tag_id_recv>>12!=9)
            {
                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;
                }
        }else{
                if(tagdist_list[taglist_pos]<g_com_map[RELAY2_DISTANCE1])
                {
                    motorstate =0;
                }else if(tagdist_list[taglist_pos]<g_com_map[RELAY2_DISTANCE2])
                {
                    motorstate =2;
                }else if(tagdist_list[taglist_pos]<g_com_map[RELAY2_DISTANCE3])
                {
                    motorstate =1;
                }else{
                    motorstate =0;
                }
        }
            
            tx_nearresp_msg[GROUP_ID_IDX] = group_id;
			tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
			tx_nearresp_msg[MESSAGE_TYPE_IDX]=NEAR_RESPONSE;
            remotesend_state = 0;
			tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|(motorstate&0xf);
			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);//ÑÓ³Ù·¢ËÍ£¬µÈ´ý½ÓÊÕ

			
			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)&&group_id==rx_buffer[GROUP_ID_IDX]) //ÅжÏÊÇ·ñΪ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;
                    u8 si;
					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µÄ¾àÀë	
					dwt_readdiagnostics(&d1);
					LOS(&d1);					
					/*--------------------------ÒÔÏÂΪ·Ç²â¾àÂß¼­------------------------*/
					//dist_cm=33000;
					getrange_success = 1;
					
					LED0_BLINK; //ÿ³É¹¦Ò»´ÎͨѶÔòÉÁ˸һ´Î
					dis_after_filter=dist_cm;
					hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10;

                        if(recpoll_len==rec_nearbase_num*4+15)
                        {
                            hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET]*10-rec_antdelay*10;
                        }
                    
					g_flag_Taggetdist[taglist_pos]=0;
                    
					if(hex_dist>-100000&&hex_dist<2000000)
					{
					if(abs(hex_dist-his_dist[taglist_pos])<dist_threshold||misdist_num[taglist_pos]>3)
					{u8 i=0;
						
                        g_Resttimer=0;
                        IWDG_Feed();
						#ifdef TDFILTER
						NewTrackingDiffUpdate(taglist_pos, (float)hex_dist);
						filter_dist = pos_predict[taglist_pos]/10;
                        filter_speed = vel_predict[taglist_pos]/10;
                        newmeasure = 1;
						#else
						filter_dist=hex_dist/10;
						#endif
						misdist_num[taglist_pos]=0;
					tagdist_list[taglist_pos] = filter_dist;	
					his_dist[taglist_pos]=hex_dist;	
                        if(hex_dist>0)
						{
                            g_Tagdist[taglist_pos]=hex_dist/10;	
                        }else{
                        //    g_Tagdist[taglist_pos]= 1;
                        }
				#ifdef USART_SINGLE_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);			
                    if(tagdist_list[taglist_pos]<=0)
                        tagdist_list[taglist_pos]=0x2ffff;
					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;
					checksum = Checksum_u16(&usart_send[2],17);
					memcpy(&usart_send[19],&checksum,2);
					if(g_com_map[MODBUS_MODE]!=1)
					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
                    tagstate_list[taglist_pos] = (button<<8)|battary;
					for(i=0;i<taglist_num-1;i++)
					{
						if( tagdist_list[i]>tagdist_list[i+1]&&tagdist_list[i+1]!=0)
						{
							int32_t tempdist;
							u16 tempid,tempstate;
							tempdist = tagdist_list[i];
							tempid = tagid_list[i];
							tempstate = tagstate_list[i];
							
							tagdist_list[i] = tagdist_list[i+1];
							tagid_list[i] = tagid_list[i+1];
							tagstate_list[i] = tagstate_list[i+1];
							
							tagdist_list[i+1] = tempdist;
							tagid_list[i+1] = tempid;
							tagstate_list[i+1] = tempstate;
						}
					}
					for(i=0;i<taglist_num;i++)
					{
						Modbus_HoldReg[i*4] = tagid_list[i];
						Modbus_HoldReg[i*4+1] = tagstate_list[i];
						memcpy(&Modbus_HoldReg[i*4+2],&tagdist_list[i],4);
					}
					}else{
					//	printf("%d",hex_dist);
						misdist_num[taglist_pos]++;
					}
				}
			}
			}else{
					//printf("%x/n",status_reg);
				dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
			}
}
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],rec_ancidlist[20],rec_ancdistlist[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;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
        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:					
//				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:	
                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+2*rec_nearbase_num],2*rec_nearbase_num);
                memcpy(&rec_antdelay,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*4],2);
            #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_num*2],&rec_ancdistlist,2*rec_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)
				{
					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;
				
			
				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);
	}
}
}