初步改好，有bug 0xfffff

zhyinch

2020-05-16 0dfe35b40cdcea88d1f4727df66feb1b4d4cf467

 Src/application/dw_app.c

@@ -15,6 +15,7 @@
 */

#include <string.h>
#include <math.h>
#include "dw_app.h"
#include "deca_device_api.h"
#include "deca_regs.h"
@@ -46,15 +47,18 @@
#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 800
#define RESP_RX_TO_FINAL_TX_DLY_UUS 850
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 600

#define DELAY_BETWEEN_TWO_FRAME_UUS 240

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


#define SPEED_OF_LIGHT 299702547

@@ -64,29 +68,13 @@
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4

#define SYNC_SEQ_IDX             5

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

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

/*------------------------------------ Variables ------------------------------------------*/
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
enum enumtagstate
{
   DISCPOLL,
   GETNEARMSG,
   NEARPOLL,
}tag_state;
static dwt_config_t config = {
   2,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
@@ -94,72 +82,39 @@
   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) */
   0,               /* 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. */
};

/* Frames used in the ranging process. See NOTE 2 below. */
static uint8_t tx_poll_msg[20] = {0};
static uint8_t tx_sync_msg[14] = {0};
//static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_resp_msg[20] = {0};
//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	
/* Frame sequence number, incremented after each transmission. */
static uint32_t frame_seq_nb = 0,frame_seq_nb2=0;	
	
/* Hold copy of status register state here for reference, so reader can examine it at a breakpoint. */
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;
	
/* Buffer to store received response message.
 * Its size is adjusted to longest frame that this example code is supposed to handle. */
#define RX_BUF_LEN       24
static uint8_t rx_buffer[RX_BUF_LEN];
	
/* Time-stamps of frames transmission/reception, expressed in device time units.
 * As they are 40-bit wide, we need to define a 64-bit int type to handle them. */
static uint8_t rx_buffer[100];
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
static uint64_t final_tx_ts;
	
/* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;

static double tof;
	
int32_t anchor_dist_last_frm[TAG_NUM_IN_SYS],his_dist[TAG_NUM_IN_SYS];   ;   
uint32_t tag_id = 0;
uint32_t tag_id_recv = 0;
uint32_t anc_id_recv = 0;
uint8_t random_delay_tim = 0;

double distance, dist_no_bias, dist_cm;

uint32_t g_UWB_com_interval = 0; 
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器

int32_t g_Tagdist[TAG_NUM_IN_SYS];
uint8_t g_flag_Taggetdist[256];
/*------------------------------------ Functions ------------------------------------------*/


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

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

/*! ------------------------------------------------------------------------------------------------------------------
 * @fn final_msg_set_ts()
 *
 * @brief Fill a given timestamp field in the final message with the given value. In the timestamp fields of the final
 *        message, the least significant byte is at the lower address.
 *
 * @param  ts_field  pointer on the first byte of the timestamp field to fill
 *         ts  timestamp value
 *
 * @return none
 */
static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
{
    int i;
@@ -228,23 +162,6 @@
        *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]>=20)
         {
            g_Tagdist[i]=0xffff;
         }
      }
   }
}

void Dw1000_Init(void)
{
@@ -252,7 +169,6 @@
     * 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);   //设置为快速模式
    dwt_initialise(DWT_LOADUCODE);//初始化DW1000
   Spi_ChangePrescaler(SPIx_PRESCALER_FAST);   //设置为快速模式

@@ -272,15 +188,18 @@
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, 4);
   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4);
   memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 4);
   memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 2);
   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
   memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 2);
   memcpy(&tx_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) 
{
@@ -292,109 +211,76 @@
    return sum;
}

uint16_t tag_time_recv[TAG_NUM_IN_SYS];
uint8_t usart_send[25];
uint8_t battary,button;
u16 tag_time_recv[TAG_NUM_IN_SYS];
u8 usart_send[25];
u8 battary,button;
extern uint8_t g_pairstart;
void tag_sleep_configuraion(void)
{
   dwt_configuresleep(0x940, 0x7);
   dwt_entersleep();
} 
uint8_t g_start_send_flag;
uint8_t g_start_sync_flag;
void SyncPoll(uint8_t sync_seq)
{
   g_start_sync_flag=1;
   dwt_forcetrxoff();
   tx_sync_msg[SYNC_SEQ_IDX]=sync_seq;
   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);
}
extern uint8_t g_start_send_flag;

uint16_t g_Resttimer;
uint8_t result;
uint8_t tag_succ_times=0;
int32_t hex_dist,hex_dist2;
uint16_t checksum;
u8 tag_succ_times=0;
int32_t hex_dist;
u16 checksum;
int8_t tag_delaytime;
extern uint16_t sync_timer;
uint16_t tmp_time,sync_count;
uint32_t time32_incr;uint32_t frame_len;
void Tag_App(void)//发送模式(TAG标签)
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)
{
	
   uint32_t frame_len;
   uint32_t final_tx_time;
   uint32_t start_poll,id;
   uint8_t i,getsync_flag=0;
   //LED0_ON;
   //dwt_forcetrxoff();
   id =  dwt_readdevid() ;
       while (DWT_DEVICE_ID != id) 
    {
         id =  dwt_readdevid() ;
    }
   g_Resttimer=0;
   u32 start_poll;
   int32_t mindist=999999,minid=-1,temp_dist;
   u8 i,getsync_flag=0;
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);      
   tag_succ_times = 0;
   tx_poll_msg[BATTARY_IDX] = Get_Battary();
   //tx_poll_msg[BUTTON_IDX] = !READ_KEY0;
   tx_poll_msg[SEQUENCE_IDX] = frame_seq_nb++;

//   GPIO_WriteBit(GPIOA, GPIO_Pin_9, Bit_RESET);
   for(i=0;i<g_com_map[MAX_REPORT_ANC_NUM];i++)
   {
   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
   tx_poll_msg[ANC_TYPE_IDX] = i;
		
   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中设置

   /* Start transmission, indicating that a response is expected so that reception is enabled automatically after the frame is sent and the delay
    * set by dwt_setrxaftertxdelay() has elapsed. */
   result=dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
   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)))//不断查询芯片状态直到成功接收或者发生错误
   {
      status_reg = dwt_read32bitreg(SYS_STATUS_ID);
//      if(time32_incr - start_poll>20)
//      NVIC_SystemReset();
   //   IdleTask();
		
   };

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

   if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
   {
      /* Clear good RX frame event and TX frame sent in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清楚寄存器标志位

      /* A frame has been received, read it into the local buffer. */
      frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;   //获得接收到的数据长度

      dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据


      /* Check that the frame is the expected response from the companion "DS TWR responder" example.
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
		
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      { uint16_t anc_id_recv;
         /* Retrieve poll transmission and response reception timestamp. */
      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&&g_com_map[DEV_ROLE])
         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)
         {
@@ -403,74 +289,40 @@
            if(sync_timer>=1010)
               {sync_timer=0;}
         }
         sync_count=(sync_timer*1000+tmp_time)/LPTIMER_LSB;
         __HAL_LPTIM_CNT_SET(&hlptim1, sync_count);
         for(i=0;i<100;i++)
         {
            lastpoll_count = slot_startcount+i*interval_count;
            if(lastpoll_count>sync_count)
            {
             __HAL_LPTIM_COMPARE_SET(&hlptim1, lastpoll_count);
               break;
            }		
         }
			

      //   TIM3->CNT=tmp_time;
//         TIM3->CNT=tmp_time;
//         if(tagslotpos>max_slotpos)
//            tagslotpos=tagslotpos%(max_slotpos+1);
//            tyncpoll_time=(tagslotpos-1)*slottime;
      }
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
			
         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 * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间，T5=T4+Treply2
         dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5

         /* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
         final_tx_ts = (((uint64_t)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线delay

         /* Write all timestamps in the final message. See NOTE 10 below. */
         final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
         final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
         final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);

         /* Write and send final message. See NOTE 7 below. */
		
         dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//将发送数据写入DW1000
         dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
			
         tag_succ_times++;
			
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送			
         tag_succ_times++;		
            LED0_BLINK;
				
               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
//               g_Tagdist[anc_id_recv]=   hex_dist;
//               g_flag_Taggetdist[anc_id_recv]=0;
               if(!g_com_map[MODBUS_MODE])
               {
               usart_send[2] = 1;//正常模式
               usart_send[3] = 17;//数据段长度
               usart_send[4] = frame_seq_nb;//数据段长度
               memcpy(&usart_send[5],&dev_id,2);
               memcpy(&usart_send[7],&rx_buffer[ANCHOR_ID_IDX],2);
				
               memcpy(&usart_send[9],&hex_dist2,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);
               }
      //         memcpy(&Modbus_HoldReg[anc_id_recv*2],&hex_dist,4);
         /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
			
         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);//清除标志位

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

         random_delay_tim = 0;
      }
      else
@@ -480,21 +332,290 @@
   }
   else
   {
      /* Clear RX error events in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
      random_delay_tim = DFT_RAND_DLY_TIM_MS;
   }
//   deca_sleep(10);
}
   dwt_entersleep();
//   if(tag_succ_times<g_com_map[MIN_REPORT_ANC_NUM])
//   {
//   //poll_timer +=time32_incr&0x7+3;
//   }
//HAL_PWR_EnterSLEEPMode(PWR_MAINREGULATOR_ON, PWR_SLEEPENTRY_WFI);
   /* Execute a delay between ranging exchanges. */
   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;
uint16_t current_count,start_count,end_count;
void NearPoll(void)
{
   static u8 mainbase_lost_count=0,flag_finalsend;
   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;
   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++;
         while(current_count<end_count||current_count>end_count+15000)
         {
               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;
                        dwt_forcetrxoff();
                        dwt_setdelayedtrxtime(final_tx_time);
                        result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送	
                        break;
                     }
                  }
                   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-((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_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);//设定发送数据长度
                              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);
                           //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);//设定发送数据长度
                           LED0_BLINK;
                              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);

                           //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);
                  current_count=HAL_LPTIM_ReadCounter(&hlptim1);
         }
         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;
   switch(tag_state)
   {
      case DISCPOLL:
         Poll();
         break;
      case GETNEARMSG:
      //   if(trygetnearmsg_times--!=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;
   }
}