v1.9 增加蜂鸣，测试1标签对10基站稳定

yincheng.zhong

2024-03-08 d67d922c6345042cb3dfaedd621a2120fe808b70

 Src/application/dw_app.c

@@ -3,7 +3,7 @@
 *  @file    main.c
 *  @brief   Double-sided two-way ranging (DS TWR) initiator example code
 *
 *         
 *
 *
 * @attention
 *
@@ -29,7 +29,9 @@
#include <stdio.h>
#include "beep.h"
#include "modbus.h"
#include "CRC.h"

//#define USART_INTEGRATE_OUTPUT
/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100
@@ -42,23 +44,23 @@
 * 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 850
#define RESP_RX_TO_FINAL_TX_DLY_UUS_POLL 1850
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 600
///* 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 410

#define DELAY_BETWEEN_TWO_FRAME_UUS 240
///* 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 DELAY_BETWEEN_TWO_FRAME_UUS 400

////#define POLL_RX_TO_RESP_TX_DLY_UUS 470
///* 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
@@ -69,141 +71,54 @@
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4

//#define _UWB_4G

enum enumtagstate
{
   DISCPOLL,
   GETNEARMSG,
   NEARPOLL,
}tag_state;
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. */
#ifdef _UWB_4G
    2,               /* Channel number. */
#else
    5,
#endif
    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_near_msg[80] = {0};

static uint32_t frame_seq_nb = 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;      //测距用的低通滤波器



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

uint32_t uwbid=0;
void Dw1000_Init(void)
{
   /* Reset and initialise DW1000.
    /* 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. */
   Spi_ChangePrescaler(SPIx_PRESCALER_SLOW);   //设置为快速模式
    Spi_ChangePrescaler(SPIx_PRESCALER_SLOW);   //设置为快速模式
    dwt_initialise(DWT_LOADUCODE);//初始化DW1000
   Spi_ChangePrescaler(SPIx_PRESCALER_FAST);   //设置为快速模式
    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);      //设置发射天线延迟

//    dwt_setrxtimeout(1000);//设定接收超时时间，0位没有超时时间
//   dwt_rxenable(0);//打开接收
//    uwbid=dwt_readdevid();
    /* 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;
   tag_state=DISCPOLL;
   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_near_msg[ANCHOR_ID_IDX], &dev_id, 2);
   memcpy(&tx_near_msg[TAG_ID_IDX], &dev_id, 2);
}	
uint16_t Checksum_u16(uint8_t* pdata, uint32_t len) 
uint16_t Checksum_u16(uint8_t* pdata, uint32_t len)
{
    uint16_t sum = 0;
    uint32_t i;
@@ -214,408 +129,30 @@
}

u16 tag_time_recv[TAG_NUM_IN_SYS];
u8 usart_send[25];
u8 usart_send[100];
u8 battary,button;
extern uint8_t g_pairstart;
void tag_sleep_configuraion(void)
{
   dwt_configuresleep(0x940, 0x7);
   dwt_entersleep();
    dwt_configuresleep(0x940, 0x7);
    dwt_entersleep();
}
extern uint8_t g_start_send_flag;

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;
u16 nearbaseid_list[10],mainbase_id;
int32_t mainbase_dist,nearbase_distlist[10];
uint8_t trygetnearmsg_times;
void Poll(void)
u32 id;
void UWB_Wkup(void)
{
   uint32_t frame_len;
   uint32_t final_tx_time,id;
   u32 start_poll;
   int32_t mindist=999999,minid=-1,temp_dist;
   u8 i,getsync_flag=0;
   id =  dwt_readdevid() ;  //0XFFFFFF BUG 初始化缺少Spi_ChangePrescaler(SPIx_PRESCALER_SLOW);   //设置为快速模式
       while (DWT_DEVICE_ID != id) 

    SPIx_CS_GPIO->BRR = SPIx_CS;
    delay_us(600);
    SPIx_CS_GPIO->BSRR = SPIx_CS;
    id =  dwt_readdevid() ;
    while (0xDECA0130!=id)
    {
         id =  dwt_readdevid() ;
        u8 iderror_count = 0;
        id =  dwt_readdevid() ;
        if(iderror_count++>100)
        {
            printf("UWB芯片ID错误\r\n");
            break;
        }
    }
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   tag_succ_times = 0;
//   GPIO_WriteBit(GPIOA, GPIO_Pin_9, Bit_RESET);
   for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
   {
   /* 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);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置

   /* 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(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] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
      {
         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;
      }
			
         memcpy(&temp_dist, &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_POLL * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间，T5=T4+Treply2
         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_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);
         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;
			
         if(temp_dist<mindist)
         {
            memcpy(&minid, &rx_buffer[ANCHOR_ID_IDX], 4);
            tag_state=GETNEARMSG;
            trygetnearmsg_times=5;
            mindist=temp_dist;
         }
         if(result==0)
         {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
         { };
      }
         /* Clear TXFRS event. */
         dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除标志位
         random_delay_tim = 0;
      }
      else
      {
         random_delay_tim = DFT_RAND_DLY_TIM_MS; //如果通讯失败，将间隔时间增加5ms，避开因为多标签同时发送引起的冲突。
      }
   }
   else
   {
   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;
   }
   mainbase_id=minid;
}

u8 FindNearBasePos(u16 baseid)
{
   u8 i;
   for(i=0;i<nearbase_num;i++)
   {
      if(baseid==nearbaseid_list[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;
   }
}
u8 anclost_times=0 , mainbase_lost_count=0;
u8 flag_finalsend,flag_getresponse;
uint16_t current_count,start_count,end_count;
void NearPoll(void)
{
	
   uint32_t temp1,temp2,dw_systime;
   uint32_t frame_len;
   uint32_t final_tx_time;
   u32 start_poll;
   u8 i,getsync_flag=0,timeout;
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   tag_succ_times = 0;
   tx_near_msg[BATTARY_IDX] = Get_Battary();
   //tx_near_msg[BUTTON_IDX] = !READ_KEY0;
   tx_near_msg[SEQUENCE_IDX] = frame_seq_nb++;
   tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
   memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
   tx_near_msg[MESSAGE_TYPE_IDX] = NEAR_POLL;	
   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);	
   dwt_writetxdata(13+2*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(13+2*nearbase_num, 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置

   for(i=0;i<nearbase_num;i++)
   nearbase_distlist[i]=0xffff;
	
   flag_finalsend=0;
   flag_getresponse=0;
   start_count=HAL_LPTIM_ReadCounter(&hlptim1);
   recbase_num=0;
   timeout=ceil((float)nearbase_num/4)+2;
   end_count=start_count+(timeout<<5);
   if(end_count>=32768)
   {end_count-=32768;}
   mainbase_dist=100000;
   mainbase_lost_count++;
   current_count=HAL_LPTIM_ReadCounter(&hlptim1);
         while(current_count<end_count||current_count>end_count+15000)
         {
            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
               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)
                     {
								
                        dwt_forcetrxoff();
                        dwt_setdelayedtrxtime(final_tx_time);
                        result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送	
                        flag_finalsend=0;
                        break;
                     }
                  }
                     current_count=HAL_LPTIM_ReadCounter(&hlptim1);
                   if(current_count>end_count&&current_count<end_count+15000)
                        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;
                        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(rec_nearbaseid==mainbase_id)
                        {
                           //////////////////////////////////时间同步
                           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;
                           ////////////////////////////
                           mainbase_lost_count=0;
                           flag_finalsend=1;
                           memcpy(&mainbase_dist,&rx_buffer[DIST_IDX],4);
                           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-((850*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_near_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX], resp_rx_ts);
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
                           tx_near_msg[MESSAGE_TYPE_IDX]=NEAR_FINAL;
                           dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000
                           dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
                           flag_getresponse=1;
//                           dwt_setdelayedtrxtime(final_tx_time);
//                        result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送	
                        }else{
                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos+1)*4], resp_rx_ts);								
                           memcpy(&nearbase_distlist[rec_nearbasepos],&rx_buffer[DIST_IDX],4);
                           dwt_writetxdata(28+nearbase_num*4, tx_near_msg, 0);//将发送数据写入DW1000
                           dwt_writetxfctrl(28+nearbase_num*4, 0);//设定发送数据长度
								


                           //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
                        }								
                     }
                  }			
               //   dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
         }
         if(flag_getresponse==0)
         {
         LED_LR_BLINK;
         }
         if(mainbase_lost_count>5)
         {
         //   tag_state=DISCPOLL;
         }
         NearAncSelect();
         if(recbase_num<MIN_ANCHOR_NUM )
         {
            anclost_times++;
            if(anclost_times>3)
            {
               tagslotpos=poll_tx_ts%(max_slotpos+1);
            }
         }else{
            anclost_times=0;
         }

   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);


}

void GetNearMsg(void)
{
   u32 start_poll,frame_len;
   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,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);

   /* 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(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数据
      {
         nearbase_num=rx_buffer[NEARBASENUM_INDEX];
         tagslotpos=rx_buffer[TAGSLOTPOS];
         memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
         slottime=ceil((nearbase_num+2)*0.3)+1;
         tyncpoll_time=tagslotpos*slottime;
         tag_state=NEARPOLL;
      }
   }
}

void Tag_App(void)//发送模式(TAG标签)
{
   //LED0_ON;
   dwt_forcetrxoff();
   g_Resttimer=0;
//   tag_state=DISCPOLL;
   switch(tag_state)
   {
      case DISCPOLL:
         Poll();
         break;
      case GETNEARMSG:
      //   if(trygetnearmsg_times--!=0)
         {
            mainbase_lost_count=0;
            GetNearMsg();
         }
//         else{
//            tag_state=DISCPOLL;
//         }
	 
         break;
      case NEARPOLL:
         //GPIO_WriteBit(EU_TX_GPIO, EU_RX_PIN, Bit_SET);
         NearPoll();
         //GPIO_WriteBit(EU_TX_GPIO, EU_RX_PIN, Bit_RESET);
         break;
   }
}