1

zhyinch

2020-11-13 67e9f19a27e24c4804ebe321c72951380637fef2

 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"
@@ -43,18 +44,22 @@

/* 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
#define POLL_TX_TO_RESP_RX_DLY_UUS 10
/* 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
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 600
#define RESP_RX_TO_FINAL_TX_DLY_UUS 510

#define POLL_RX_TO_RESP_TX_DLY_UUS 420
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 1000

#define DELAY_BETWEEN_TWO_FRAME_UUS 240

#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

@@ -64,35 +69,18 @@
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4

#define SYNC_SEQ_IDX             5
//common
#define GROUP_ID_IDX               0
#define ANCHOR_ID_IDX             1
#define TAG_ID_IDX                5
#define MESSAGE_TYPE_IDX          9	

//Poll
#define ANC_TYPE_IDX                14
#define BATTARY_IDX                  15
#define BUTTON_IDX                  16
#define SEQUENCE_IDX               17
//respose
#define DIST_IDX                      10
#define ANCTIMEMS             14
#define ANCTIMEUS             16
#define ANCSEND_INTERVAL      18

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

/*------------------------------------ Variables ------------------------------------------*/
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
#define STARTPOLL  DISCPOLL
enum enumtagstate
{
   REGPOLL,
   DISCPOLL,
   GETNEARMSG,
   NEARPOLL,
}tag_state=STARTPOLL;
static dwt_config_t config = {
   2,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
   DWT_PLEN_128,    /* Preamble length. */
   DWT_PLEN_64,    /* 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. */
@@ -101,67 +89,34 @@
   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[24] = {0};
	
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_final_msg[60] = {0};
static uint8_t tx_resp_msg[22] = {0};
//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	
/* Frame sequence number, incremented after each transmission. */
static uint32_t frame_seq_nb = 0,frame_seq_nb2=0;	
	
/* Hold copy of status register state here for reference, so reader can examine it at a breakpoint. */
 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];
@@ -176,16 +131,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];
@@ -200,17 +145,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;
@@ -230,23 +164,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)
{
@@ -254,9 +171,9 @@
     * 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
@@ -274,19 +191,22 @@
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[GROUP_ID_IDX], &group_id, 1);
      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, 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) 
{
@@ -298,59 +218,97 @@
    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,current_slottimes;
uint32_t time32_incr;
int32_t ancsync_time;
uint32_t frame_len;
int32_t count_offset,nextpoll_delaytime;
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 mainbase_id;
int32_t mainbase_dist;
uint8_t trygetnearmsg_times;
u16 nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM];
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
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 = HAL_LPTIM_ReadCounter(&hlptim1);//GetRandomValue();
   for(i=temp_value%32;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;
}
void MODBUS_Poll(void)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   uint32_t start_poll,id;
   uint8_t i,getsync_flag=0;
   u32 start_poll;
   u8 i,getsync_flag=0;
   u8 bat_percent;
   //LED0_ON;
   //dwt_forcetrxoff();
   id =  dwt_readdevid() ;
       while (DWT_DEVICE_ID != id) 
    {
         id =  dwt_readdevid() ;
    }
   g_Resttimer=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] = bat_percent;
   //tx_poll_msg[BUTTON_IDX] = !READ_KEY0;
   tx_poll_msg[SEQUENCE_IDX] = frame_seq_nb++;

   if(bat_percent>100)
      bat_percent=100;
   tx_poll_msg[BATTARY_IDX] = bat_percent;//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. */
@@ -361,22 +319,19 @@

   /* 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;
   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();
   { 
      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)//如果成功接收
@@ -394,7 +349,7 @@
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
      
      if (rx_buffer[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      { uint16_t anc_id_recv,current_count,rec_com_interval;
      { u16 anc_id_recv,rec_com_interval;
         /* Retrieve poll transmission and response reception timestamp. */
         poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
         resp_rx_ts = get_rx_timestamp_u64();                              //获得RESPONSE接收时间T4
@@ -412,20 +367,19 @@
            if(sync_timer>=1010)
               {sync_timer=0;}
         }
			

      //   TIM3->CNT=tmp_time;
      }
         memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
      //   memcpy(&hex_dist2, &rx_buffer[DIST_IDX], 4);
         memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
         memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL],  2);
         if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
         {
            g_com_map[COM_INTERVAL]=rec_com_interval;
            save_com_map_to_flash();
            delay_ms(100);
            SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //软复位回到bootloader   
         }
      //   memcpy(&rec_com_interval,&rx_buffer[ANCSEND_INTERVAL],  2);
//         if(rec_com_interval>4&&rec_com_interval!=g_com_map[COM_INTERVAL])
//         {
//            g_com_map[COM_INTERVAL]=rec_com_interval;
//            save_com_map_to_flash();
//         //   delay_ms(100);
//            SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //软复位回到bootloader   
//         }
		
         /* 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
@@ -443,53 +397,34 @@
         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);//设定为延迟发送
		
         ancsync_time=((sync_timer+0)*1000+tmp_time);
         current_count=HAL_LPTIM_ReadCounter(&hlptim1);
      //   count_offset=sync_count-current_count-143;
      //   current_slottimes=(ancsync_time-10000)/(g_com_map[COM_INTERVAL]*1000);
         nextpoll_delaytime=tyncpoll_time*1000+g_com_map[COM_INTERVAL]*1000-((ancsync_time-10000)%(g_com_map[COM_INTERVAL]*1000))-5150;
         if(abs(ancsync_time-910000)<1000)
         {
         nextpoll_delaytime+=10000;
         }		
         if(nextpoll_delaytime<2000)
         {
            nextpoll_delaytime+=g_com_map[COM_INTERVAL]*1000;
         }
         lastpoll_count= current_count+(nextpoll_delaytime)/LPTIMER_LSB;
         if(lastpoll_count>LPTIMER_1S_COUNT)
            lastpoll_count-=LPTIMER_1S_COUNT;
         __HAL_LPTIM_COMPARE_SET(&hlptim1, lastpoll_count);
			
//         printf("ancsync_time: %u     \r\n ",ancsync_time);
//         printf("current_slottimes: %u   ",current_slottimes);
//         printf("nextpoll_delaytime: %u   ",nextpoll_delaytime);
//         printf("current_count: %u   ",current_count);
//         printf("lastpoll_count: %u",lastpoll_count);
		
         
         tag_succ_times++;
         
            LED0_BLINK;
            
               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
//               g_Tagdist[anc_id_recv]=   hex_dist;
//               if(hex_dist2!=0xffff)
//               {	
//               g_Tagdist[anc_id_recv]=   hex_dist2;
//               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);
               }
//					
//               if(!g_com_map[MODBUS_MODE])
//               {
//               hex_dist2 = hex_dist2;		
//               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] = bat_percent;
//               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(result==0)
@@ -516,15 +451,573 @@
   }
//   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);
//   dwt_entersleep();


   /* Execute a delay between ranging exchanges. */
	
      delay_ms(30);

}
int32_t mainbase_dist,nearbase_switchdistlist[MAX_NEARBASE_NUM],nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM],ancsync_time,nextpoll_delaytime,offsettimeus;
u8 anclost_times=0 , mainbase_lost_count=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
u8 flag_finalsend,flag_getresponse,flag_rxon;
uint16_t current_count,start_count,end_count,lastsync_timer;
extern u16 bigslot_num;
u8 nearbase_num=0,last_nearbase_num,next_nearbase_num,last_slotnum, para_update,para_len,stationary_flag=0;
u32 rec_tagpos_binary;
int16_t offset=4700,temptimer;
u8 motor_state,rec_remotepara_state,rec_remotepara[80];
void Poll(void)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   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;
	
   for(i=0;i<3;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);
                           current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//                           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;}
                           }
      }
			
         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_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;
			
                           ancsync_time=((sync_timer)*1000+tmp_time);	
                           last_slotnum=current_slotnum;
                           temptimer = sync_timer-10;
                           if(temptimer<0)
                           {temptimer+=1000;}
                           current_slotnum=((float)temptimer/(SLOTTIME_MS*bigslot_num))+1;
                           if(current_slotnum==last_slotnum-1)
                           {flag_getresponse=1;}
                           lastsync_timer=sync_timer;
                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
                           SetNextPollTime(tagslotpos);
		
         if(temp_dist<mindist&&temp_dist!=0)
         {
            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;
//   mainbase_id = 0x9804;
}
u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;
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];
         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],nearbase_num*2);
         //slottime=ceil((nearbase_num+2)*0.3)+1;
         //tyncpoll_time=tagslotpos*slottime;
         //tyncpoll_time=(g_com_map[DEV_ID]%max_slotpos)*slottime;	
         tag_state=NEARPOLL;
      }
   }else{
         tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
   }
}
extern u8 userkey_state;
uint8_t changemainbase_count=0,gotosleep_flag;
uint32_t frame_len;
void NearPoll(void)
{
	
   uint32_t temp1,temp2,dw_systime;

   uint32_t final_tx_time;
   u32 start_poll;
   u8 i,j,getsync_flag=0,timeout;
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   tag_succ_times = 0;
	
   if(next_nearbase_num>=MAX_NEARBASE_NUM)
   {
      next_nearbase_num = MAX_NEARBASE_NUM-1;
   }
   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);

   recbase_num=0;
//   motor_state=0;
	
   tx_near_msg[BATTARY_IDX] = bat_percent;
   tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2;
   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;	
	
   tx_near_msg[NEARP_TAGFREQ_INDEX] = tag_frequency;	
   tx_near_msg[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;	

   memcpy(&tx_near_msg[ANCHOR_ID_IDX],&mainbase_id,2);	
   dwt_writetxdata(21+2*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(21+2*nearbase_num, 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
	
   tx_near_msg[TAGCONFIGSUCCESS_INDEX] =0;
   para_update = 0;
   flag_finalsend=0;
   flag_rxon=1;
   flag_getresponse=0;
   start_count=HAL_LPTIM_ReadCounter(&hlptim1);
   recbase_num=0;
   timeout=ceil((float)nearbase_num*SLOT_SCALE)+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)))//不断查询芯片状态直到成功接收或者发生错误
               {
                     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);//清楚寄存器标志位
                     dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);//设定接收超时时间，0位没有超时时间
                     dwt_rxenable(0);//打开接收
                     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_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);
                           current_count=HAL_LPTIM_ReadCounter(&hlptim1);
                              dwt_forcetrxoff();
                           final_tx_time = (resp_rx_ts + ((RESP_RX_TO_FINAL_TX_DLY_UUS) * UUS_TO_DWT_TIME)) >> 8;
                           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+nearbase_num*4], 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);//设定发送数据长度
                           dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5
                           result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送	
                           userkey_state = !GET_USERKEY;

                           memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                           mainbase_dist=temp_dist;
                           if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
                              motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;
                           rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>4;
									
                           mainbase_lost_count=0;
                           flag_finalsend=1;
                           memcpy(&mainbase_dist,&rx_buffer[DIST_IDX],4);
									
                           if(rec_remotepara_state&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2))
                           {
                              para_update = 1;
                              tx_near_msg[TAGCONFIGSUCCESS_INDEX] = 1;
                              para_len = frame_len-22;
                              memcpy(rec_remotepara,&rx_buffer[REMOTEPARA_INDEX],para_len);
                           }
                           flag_getresponse=1;								
                           //时间同步
                        tmp_time=tmp_time+450;
                           if(tmp_time>999)
                           {
                              tmp_time-=999;
                              sync_timer++;
                              if(sync_timer>=1000)
                                 {sync_timer=0;}
                           }
                           ancsync_time=((sync_timer)*1000+tmp_time);	
                           last_slotnum=current_slotnum;
                           current_slotnum=((float)(sync_timer*10)/(SLOTTIME_MS*bigslot_num))+1;
                           if(current_slotnum==last_slotnum-1)
                           {flag_getresponse=1;}
                           lastsync_timer=sync_timer;
                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
                           SetNextPollTime(tagslotpos);
                           if(result==0)
                                 {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
                                 { };}								
                              break;
                        }else{
									
                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);									
                           exsistbase_list[rec_nearbasepos]=KEEP_TIMES;
                           memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                           memcpy(&nearbase_switchdistlist[rec_nearbasepos],&rx_buffer[NR_NEARSWITCH_DISTANCE],2);
                           nearbase_distlist[rec_nearbasepos]=temp_dist;								
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
                           if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]!=0&&temp_dist!=0)
                           {
                              mainbase_id = rec_nearbaseid;
                              tag_state = GETNEARMSG;
                           }
								
                  }								
                     }
                  }else{
                  dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
                  if(recbase_num!=nearbase_num+1)
                     {dwt_rxenable(0);   }
                     //                  if(flag_rxon)
//                     {
                  //      dwt_rxenable(0);	
//                     }								
                  }			
               //   dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
         }
         dwt_forcetrxoff();
dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
      j=0;
         if(exsistbase_list[0]==0)
         {
//            SetNextPollTime(tagslotpos);
         }
      //   tyncpoll_time=0;
//         next_nearbase_num=0;
//         for(i=0;i<nearbase_num;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;
//               }			
//            }
//         }

//         if(true_nearbase_distlist[0]<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST&&true_exsistbase_list[0])
//         {
//            changemainbase_count++;		
//         }else{
//            changemainbase_count = 0;
//         }
//         if(changemainbase_count>3)
//         {
//            mainbase_id = true_nearbase_idlist[0];
//            tag_state = GETNEARMSG;
//         }
//         if(mainbase_lost_count>3)
//         {
//            tag_state = DISCPOLL;
//         }
//            for (i=0;i<nearbase_num;i++)
//   {
//      if(nearbaseid_list[i]==0)
//      {
//         tag_state = GETNEARMSG;
//      }
//	
//   }
//串口输出距离			
//         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]==0xa)
//            {
//               memcpy(&usart_send[4+6*report_num],&nearbaseid_list[i],2);
//               memcpy(&usart_send[6+6*report_num],&nearbase_distlist[i],4);
//               report_num++;
//            }
//         }
//         for(i=0;i<MAX_NEARBASE_NUM;i++)
//         {
//            nearbase_distlist[i]=0x1ffff;
//         }
         if(para_update)
         {
         uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;
            pack_msgtype = rec_remotepara[0];
            pack_index = rec_remotepara[1];
            pack_length = rec_remotepara[2];
            if(pack_msgtype==2)
            {
                  memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
               //返回一个error状态
               //SendComMap(pack_datalen,pack_index);
               save_com_map_to_flash();
               delay_ms(100);
               NVIC_SystemReset();
            }
         }
   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_RESET);

}
extern uint8_t module_power,imu_enable,motor_enable;

void Registor_Poll(void)
{
   static u8 regpoll_count=0;
      tag_frequency = REGISTER_FREQUENCY;
      bigslot_num = TOTAL_SLOTNUM/tag_frequency;
      regpoll_count++;
      if(regpoll_count%2)
      {
         if(tagslotpos--<2)
            tagslotpos=TOTAL_SLOTNUM;
      }
      SetNextPollTime(tagslotpos);
      dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		

      tx_near_msg[REGP_TAGSTATE_INDEX] = !GET_USERKEY<<2|imu_enable<<1|motor_enable;
      tx_near_msg[MESSAGE_TYPE_IDX] = REG_POLL;	
      tx_near_msg[REGP_FREQUENCY_INDEX] = 1000/g_com_map[COM_INTERVAL];
      tx_near_msg[REGP_POWER_INDEX] = module_power;
      memcpy(&tx_near_msg[REGP_VERSION_INDEX],&g_com_map[VERSION],2);	
      memcpy(&tx_near_msg[REGP_IMUTHRES_INDEX],&g_com_map[IMU_THRES],2);
      memcpy(&tx_near_msg[REGP_NOMOVESLEEPTIME_INDEX],&g_com_map[NOMOVESLEEP_TIME],2);
      dwt_writetxdata(21, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
      dwt_writetxfctrl(21, 0);//设置超宽带发送数据长度
      dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到成功接收或者发生错误
   { 
      IdleTask();
		
   };
      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] == REG_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)) //判断接收到的数据是否是response数据
                     {   u16 rec_nearbaseid,rec_nearbasepos;
								
                        memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
                        tag_frequency = rx_buffer[REGR_TAGFREQ_INDEX];
                        bigslot_num = TOTAL_SLOTNUM/tag_frequency;
                        tagslotpos = rx_buffer[REGR_TAGSLOTPOS_INDEX];
                           //////////////////////////////////时间同步
                           memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
                           memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
                           current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//                           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;}
                           }
                           ancsync_time=((sync_timer)*1000+tmp_time);	
                           last_slotnum=current_slotnum;
                           temptimer = sync_timer-10;
                           if(temptimer<0)
                           {temptimer+=1000;}
                           current_slotnum=((float)temptimer/(SLOTTIME_MS*bigslot_num))+1;
                           if(current_slotnum==last_slotnum-1)
                           {flag_getresponse=1;}
                           lastsync_timer=sync_timer;
                           offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
//                           SetNextPollTime(tagslotpos);
                           mainbase_id=rec_nearbaseid;
                           nearbase_num=rx_buffer[NEARBASENUM_INDEX];
                           memcpy(nearbaseid_list,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
                           tag_state = NEARPOLL;
                        }
                     }
}
u32 id;
void Tag_App(void)//发送模式(TAG标签)
{
	
   //LED0_ON;
   id =  dwt_readdevid() ;
       while (DWT_DEVICE_ID != id) 
    {
         Dw1000_Init();
         id =  dwt_readdevid() ;
    }
   switch(tag_state)
   {
      case REGPOLL:
         LED_LG_ON;		
         Registor_Poll();			
         LED_LG_OFF;
         break;
      case DISCPOLL:
         LED_LG_ON;		
         Poll();
         LED_LG_OFF;
         break;
      case GETNEARMSG:
         trygetnearmsg_times--;	
         LED_LG_ON;		
            GetNearMsg();	 
      LED_LG_OFF;
      if(trygetnearmsg_times==0)
      {
         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;
   }
   dwt_entersleep();
}