#include "dw_app.h"
#include "ADC.h"
enum enumtagstate
{
    DISCPOLL,
    GETNEARMSG,
    NEARPOLL,
} tag_state=GETNEARMSG;
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[90] = {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_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);
    }
}
extern int32_t tagdist_list[TAG_NUM_IN_SYS];
void TagDistClear(void)
{
    static uint16_t clear_judge_cnt;
    uint16_t i;
    for(i=0; i<255; i++)
    {
        g_flag_Taggetdist[i]++;
        if(g_flag_Taggetdist[i]>=2)
        {
            tagdist_list[i]=0x1ffff;
        }
    }
}
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. */
    status_reg = dwt_read32bitreg(SYS_STATUS_ID);
    dwt_initialise(DWT_LOADUCODE);//³õʼ»¯DW1000
    status_reg = dwt_read32bitreg(SYS_STATUS_ID);
    Spi_ChangePrescaler(SPIx_PRESCALER_FAST);	//ÉèÖÃΪ¿ìËÙģʽ

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

    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);

    /* Apply default antenna delay value. See NOTE 1 below. */
    dwt_setrxantennadelay(RX_ANT_DLY);		//ÉèÖýÓÊÕÌìÏßÑÓ³Ù
    dwt_settxantennadelay(TX_ANT_DLY);		//ÉèÖ÷¢ÉäÌìÏßÑÓ³Ù

    /* 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. */
    //ÉèÖýÓÊÕ³¬Ê±Ê±¼ä
}
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[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[150],usart_send_anc[100];
u8 battary,button,tag_frequency,tag_slotpos;
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)
{   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],&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);
    if(result==0)
    {
        while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//²»¶Ï²éѯоƬ״ֱ̬µ½·¢ËÍÍê³É
        { };
    }
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//Çå³þ±ê־λ

    dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
    dwt_rxenable(0);//´ò¿ª½ÓÊÕ
}
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;
}
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((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;
}

u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32];

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];
    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;
}
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;
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);//ÑÓ³Ù·¢ËÍ£¬µÈ´ý½ÓÊÕ


    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] == DISCOVERFINAL&&!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(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,userdatasend_flag[TAG_NUM_IN_SYS];
u8 finalsend=0;
u16 signalpower_list[TAG_NUM_IN_SYS];
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;
u32 errortimes,goodtimes;
u8 tagpossend_flag;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase  1 first near_base
{

    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_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);//½ÓÊÕ³¬Ê±Ê±¼ä
    //dwt_readdiagnostics(&d1);
    if(tagpos[taglist_pos].tagid==tag_id_recv&&userdatasend_flag[taglist_pos] == 1)
    {
        memcpy(&tx_nearresp_msg[TAGPOS_INDEX],&tagpos[taglist_pos],tagpos[taglist_pos].datalen+3);
        tagpossend_flag = 1;
    } else {
        tagpossend_flag = 0;
    }
    if(new_tagid)
    {
        tagdist_list[taglist_pos]=0x1ffff;
    }
    memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
    memcpy(&tx_nearresp_msg[ANC_SIGNALPOWER],&signalpower_list[taglist_pos],2);
    tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
    tx_nearresp_msg[MESSAGE_TYPE_IDX]=POS_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++)
        {
            if(pwtag.groupid[i][0]<=tag_id_recv&&tag_id_recv<=pwtag.groupid[i][1])
            {
                remotesend_state=1;
                tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
                remotetag_paralen = 2+3;
                remotetag_para[0] = 2;
                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(24+remotetag_paralen, tx_nearresp_msg, 0);//дÈë·¢ËÍÊý¾Ý
                dwt_writetxfctrl(24+remotetag_paralen, 0);//É趨·¢Ëͳ¤¶È
                remotesend_state=0;
                finalsend = 1;
                break;
            }
        }
    } else {

    }

    if(finalsend == 0)
    {
        memcpy(&tx_nearresp_msg[MAXRANGE_DISTANCE],&g_com_map[MAXRANGDIST_INDEX],2);
        dwt_writetxdata(28+tagpossend_flag*(tagpos[taglist_pos].datalen+3), tx_nearresp_msg, 0);//§Õ????????
        dwt_writetxfctrl(28+tagpossend_flag*(tagpos[taglist_pos].datalen+3), 0);//?څ???????
    }

//			 if(remotesend_state&&tag_id_recv==configremotetagID)
//			{
//			 	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);//É趨·¢Ëͳ¤¶È
//				waittagconfig_reponse=1;
//			}else{

    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)))///²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
        { };
    } 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] == POS_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //ÅжÏÊÇ·ñΪFinal°ü
        {

            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);

            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
            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µÄ¾àÀë
            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];
            userdatasend_flag[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)
                {
                    goodtimes++;
                    misdist_num[taglist_pos]=0;
                    tagdist_list[taglist_pos] = hex_dist;
                    his_dist[taglist_pos]=hex_dist;
                    g_Tagdist[taglist_pos]=hex_dist;
                    signalpower_list[taglist_pos] = ((uint8_t)firstpath_power<<8)|(uint8_t)((rx_power-firstpath_power)*10);
//				#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 {
                errortimes++;
                usart_send[13] = battary;
            }
            if(rx_buffer[TAGCONFIGSUCCESS_INDEX]==1)
            {

                remotesend_state = 0;
                usart_send[2] = 7;//Õý³£Ä£Ê½
                usart_send[3] = 5;//Êý¾Ý¶Î³¤¶È
                memcpy(&usart_send[4],&tag_id_recv,2);
                usart_send[6] = 1;//Êý¾Ý¶Î³¤¶È
                checksum = Checksum_u16(&usart_send[2],5);
                memcpy(&usart_send[7],&checksum,2);
                UART_PushFrame(usart_send,9);
            }
        }
    } else {
#ifdef DEBUG_INF
        printf("final°üʧ°Ü£º%x/n",status_reg);
#endif
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }
}
void Anchor_Start(void)
{
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
    g_start_sync_flag=0;

    dwt_setrxtimeout(1000);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
    dwt_rxenable(0);//´ò¿ª½ÓÊÕ
//    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();
//	};
}
extern int32_t intheight;
extern u16 synclost_count;
uint32_t current_syncid=0xffffffff,synclost_timer;
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],rec_taganc_signalpower_list[11];
int32_t recnearbaselist_dist[30];
int16_t rec_tagheight;
u16 timm = 0;
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)))//²»¶Ï²éѯоƬ״ֱ̬µ½½ÓÊճɹ¦»òÕß³öÏÖ´íÎó
//	{
//		//IdleTask();
//	};
//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 0);
    status_reg  = dwt_read32bitreg(SYS_STATUS_ID);
    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;//»ñµÃ½ÓÊÕÊý¾Ý³¤¶È
        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])
        {
            uint16_t checksum;
        case DISCOVERPOLL:
            if (anchor_type == rx_buffer[ANCHOR_ID_IDX]&&g_com_map[NEARSWITCH_DISTANCE_INDEX] != 0)
            {
                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];
                if(rec_nearbase_num>=30)
                {
                    break;
                }
                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_num*2],rec_nearbase_num*4);
                Anchor_RecPoll();
            }
            break;
        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];
                if(rec_nearbase_num>=30)
                {
                    break;
                }
                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_num*2],rec_nearbase_num*4);
                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_num*2],&recnearbaselist_dist,4*rec_nearbase_num);
                memcpy(&usart_send[17+rec_nearbase_num*6],zero_matrix,2*rec_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(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)-27;
                sync_timer = (sync_seq*325/1000);
                synclost_timer=0;
                if(g_com_map[BASESYNCSEQ]==2)
                    SyncPoll(sync_seq);
            }
            break;
        case POS_MSG:
            if(anc_id_recv==dev_id)
            {
                rx_buffer[TAGSLOTPOS]=taglist_pos;
                tx_near_msg[MESSAGE_TYPE_IDX] = POS_MSG;
                memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
                memcpy(&tx_near_msg[NEARMSG_EMPTYSLOTPOS_INDEX],&tagpos_binary,4);
                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);
            }
            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;
                }

            }
            if(i==MAX_REGTAGNUM&&g_com_map[NEARSWITCH_DISTANCE_INDEX] != 0)
            {
                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 POS_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)
            {
                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>30)
            {
                break;
            }
            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*8+6],2);
            memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
            memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4+4);
            memcpy(&rec_taganc_signalpower_list,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],rec_nearbase_num*2+2);
            if(anc_id_recv == dev_id||g_com_map[OUTPUTALLNEARPOLLMSG] == 1 )
            {
                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_num*2],&recnearbaselist_dist,4*rec_nearbase_num+4);
                memcpy(&usart_send[19+rec_nearbase_num*6+4],&rec_taganc_signalpower_list,2*rec_nearbase_num+2);

                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);
//					UART_CheckSend();

                timm++;
            }
            //////////////////////////////////////////////////////////////


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

                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);
//					}
//                    if(tag_id_recv==0x4008)
//                    printf("±êÇ©¸ß¶È: %d,»ùÕ¾¸ß¶È: %d,¸ß¶È²î£º %d. \r\n",rec_tagheight,intheight,intheight-rec_tagheight);

            break;
        default:

            break;
        }
    } else {
#ifdef DEBUG_INF
        printf("poll°üʧ°Ü£º%x/n",status_reg);
#endif
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }
    UART_CheckSend();
    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);
    dwt_setrxtimeout(0);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
    dwt_rxenable(0);//´ò¿ª½ÓÊÕ

}