2.59改为最新的ss定位代码，去掉双击关闭uwb

WXK

2024-11-29 4353e7b92b65e9a558adfbbbb75bc9afdfda69b6

 Src/application/dw_app.c

@@ -38,48 +38,48 @@
#define TX_ANT_DLY 0
#define RX_ANT_DLY 32899

/* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
 * 1 uus = 512 / 499.2 祍 and 1 祍 = 499.2 * 128 dtu. */
#define UUS_TO_DWT_TIME 65536
///* UWB microsecond (uus) to device time unit (dtu, around 15.65 ps) conversion factor.
// * 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 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 1000
///* 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 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 1000

/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 4000
///* Receive response timeout. See NOTE 5 below. */
//#define RESP_RX_TIMEOUT_UUS 4000

#define DELAY_BETWEEN_TWO_FRAME_UUS 240
//#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 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
//#define SPEED_OF_LIGHT 299702547

/* Indexes to access some of the fields in the frames defined above. */
#define FINAL_MSG_POLL_TX_TS_IDX 10
#define FINAL_MSG_RESP_RX_TS_IDX 14
#define FINAL_MSG_FINAL_TX_TS_IDX 18
#define FINAL_MSG_TS_LEN 4
///* Indexes to access some of the fields in the frames defined above. */
//#define FINAL_MSG_POLL_TX_TS_IDX 10
//#define FINAL_MSG_RESP_RX_TS_IDX 14
//#define FINAL_MSG_FINAL_TX_TS_IDX 18
//#define FINAL_MSG_TS_LEN 4

#define STARTPOLL  REGPOLL//#define SWITCHBASE_DIST
#define SWITCHBASE_ZHUANDIAN
#define SWITCHBASE_DIST
enum enumtagstate
{
   REGPOLL,
   DISCPOLL,
   GETNEARMSG,
   NEARPOLL,
  SINGLEPOLL,
}tag_state=STARTPOLL;
//#define STARTPOLL  REGPOLL//#define SWITCHBASE_DIST
//#define SWITCHBASE_ZHUANDIAN
//#define SWITCHBASE_DIST
//enum enumtagstate
//{
//   REGPOLL,
//   DISCPOLL,
//   GETNEARMSG,
//   NEARPOLL,
//  SINGLEPOLL,
//}tag_state=STARTPOLL;
//#define   _UWB_4G
static dwt_config_t config = {
#ifdef   _UWB_4G
@@ -97,80 +97,80 @@
   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};
 uint8_t tx_near_msg[180] = {0};
//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};
// uint8_t tx_near_msg[180] = {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;
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 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;
//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;      //测距用的低通滤波器


void GetNearMsg(void);
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;
}
//void GetNearMsg(void);
//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 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_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);
    }
}
//static void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts)
//{
//    int i;
//    *ts = 0;
//    for (i = 0; i < FINAL_MSG_TS_LEN; i++)
//    {
//        *ts += ts_field[i] << (i * 8);
//    }
//}

void Dw1000_Init(void)
{
@@ -195,26 +195,6 @@
     * 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[GROUP_ID_IDX], &group_id, 1);
   memcpy(&tx_final_msg[GROUP_ID_IDX], &group_id, 1);
   memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);
	
   memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 2);
   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
   memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 2);
   memcpy(&tx_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 sum = 0;
@@ -224,57 +204,174 @@
    sum = ~sum;
    return sum;
}

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)
int16_t target_offsettime = 850;
int32_t error_time;
u32 temptime,delaytime,expect_anctime;
int16_t poll_offsettime=-6900,clockoffset;
extern u16 slotpos_intoatl, slotpos;
extern u16 bigslot_num;
u8 pd_i;
extern u16 tagslotpos;
void NextPollDelay(u32 anchor_time)
{
   dwt_configuresleep(0x940, 0x7);
   dwt_entersleep();
    //tagslotpos = 0;
    error_time = anchor_time-expect_anctime;
    if(error_time<2000&&error_time>0)
        clockoffset += (target_offsettime-error_time)*0.1;
    for(pd_i=0; pd_i<tag_frequency; pd_i++)
    {
        slotpos_intoatl = pd_i*bigslot_num+tagslotpos;
        temptime = (slotpos_intoatl*SLOTTIME_MS+100)*100;
        if(anchor_time<temptime-5000)
        {
            current_slotnum = pd_i;
            delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
            expect_anctime = temptime;
            return ;
        }
    }
    temptime = (tagslotpos*SLOTTIME_MS+100+10000)*100;
    expect_anctime = temptime-1000000;
    delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
    return ;
}
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 mainbase_id;
int32_t mainbase_dist,base_mindist;
uint8_t trygetnearmsg_times;
u16 mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],nearbaseid_list0[MAX_NEARBASE_NUM],nearbaseid_list[MAX_NEARBASE_NUM],nearbaseid_list2[MAX_NEARBASE_NUM];
u8 FindNearBasePos(u16 baseid)
u16 lpcount,poll_startcount,last_lpcount;
u16 waketopolltimeus = 3200;
#define BASENUM_COMTIME 244
u32 last_anchor_time;
static u16 current_count;
void SetLPTimer(u32 anchor_time,u8 nearbase_num)
{
   u8 i;
   for(i=0;i<nearbase_num;i++)
   {
      if(baseid==nearbaseid_list[i])
         return i;
   }

    NextPollDelay(anchor_time);
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    lpcount = current_count+delaytime/LPTIMER_LSB+(10-nearbase_num)*8;
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}
u8 recbase_num=0;
#define CHANGE_BASE_THRESHOLD  5
uint8_t GetRandomValue(void)
void NextSlotDelayMs(int16_t delayms)
{
   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;
    current_slotnum++;
    if(current_slotnum>=tag_frequency)
    {
        current_slotnum = 0;
        lpcount = last_lpcount+31457/tag_frequency+1311+32.767*delayms;//(float)(delayms+40)*32.767 ;//(200/tag_frequency+delayms)*4.8*1000/LPTIMER_LSB
    } else {
        lpcount = last_lpcount+31457/tag_frequency+32.767*delayms;//(float)(200/tag_frequency*48+delayms*10)*3.3 ;
    }
    if(lpcount>=32768)
    {
        lpcount -=32768;
    }
    poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
    if(poll_startcount>=32768)
    {
        poll_startcount -=32768;
    }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
    last_lpcount = lpcount;
}
void WaitUntilPollStart(void)
{
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    while(current_count<poll_startcount||current_count>poll_startcount+16384)
    {
        current_count=HAL_LPTIM_ReadCounter(&hlptim1);
        if(current_count<poll_startcount-300)
        {
            break;
        }
    }
}
//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_final_msg[GROUP_ID_IDX], &group_id, 1);
//   memcpy(&tx_resp_msg[GROUP_ID_IDX], &group_id, 1);
//	
//   memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 2);
//   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
//   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
//   memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 2);
//   memcpy(&tx_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 sum = 0;
//    uint32_t i;
//    for(i=0; i<len; i++)
//        sum += pdata[i];
//    sum = ~sum;
//    return sum;
//}

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

//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 mainbase_id;
//int32_t mainbase_dist,base_mindist;
//uint8_t trygetnearmsg_times;
//u16 mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM],nearbaseid_list0[MAX_NEARBASE_NUM],nearbaseid_list[MAX_NEARBASE_NUM],nearbaseid_list2[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;
@@ -296,998 +393,998 @@
   }
return max_slotpos-1;
}
void MODBUS_Poll(void)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   u32 start_poll;
   u8 i,getsync_flag=0;
   u8 bat_percent;
   //LED0_ON;
   //dwt_forcetrxoff();
   g_Resttimer=0;
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   tag_succ_times = 0;
//void MODBUS_Poll(void)
//{
//   uint32_t frame_len;
//   uint32_t final_tx_time;
//   u32 start_poll;
//   u8 i,getsync_flag=0;
//   u8 bat_percent;
//   //LED0_ON;
//   //dwt_forcetrxoff();
//   g_Resttimer=0;
//    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
//    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
//   tag_succ_times = 0;

   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. */
   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);//设置超宽带发送数据长度
//   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. */
//   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);//设置超宽带发送数据长度

   /* 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. */
   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)))//不断查询芯片状态直到成功接收或者发生错误
   { 
      IdleTask();
		
   };
//   /* 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. */
//   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)))//不断查询芯片状态直到成功接收或者发生错误
//   { 
//      IdleTask();
//		
//   };

   /* Increment frame sequence number after transmission of the poll message (modulo 256). */
   if(status_reg==0xffffffff)
   {
      NVIC_SystemReset();
   }
//   /* Increment frame sequence number after transmission of the poll message (modulo 256). */
//   if(status_reg==0xffffffff)
//   {
//      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);//清楚寄存器标志位
//   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;   //获得接收到的数据长度
//      /* 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);   //读取接收数据
//      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[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      { 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
			
         if(getsync_flag==0&&g_com_map[DEV_ROLE])
         {
            getsync_flag=1;
         memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
         memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],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;
      }
      //   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])
//      /* 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[GROUP_ID_IDX] == group_id&&rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
//      { 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
//			
//         if(getsync_flag==0&&g_com_map[DEV_ROLE])
//         {
//            g_com_map[COM_INTERVAL]=rec_com_interval;
//            save_com_map_to_flash();
//         //   delay_ms(100);
//            SCB->AIRCR = 0X05FA0000|(unsigned int)0x04; //软复位回到bootloader   
//            getsync_flag=1;
//         memcpy(&sync_timer,&rx_buffer[ANCTIMEMS],2);
//         memcpy(&tmp_time,&rx_buffer[ANCTIMEUS],2);
//         tmp_time=tmp_time+450;
//         if(tmp_time>999)
//         {
//            tmp_time-=999;
//            sync_timer++;
//            if(sync_timer>=1010)
//               {sync_timer=0;}
//         }
		
         /* 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
//      //   TIM3->CNT=tmp_time;
//      }
//      //   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   
////         }
//		
//         /* 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
//         /* 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 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++;
			
            LED0_BLINK;
				
               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
//               if(hex_dist2!=0xffff)
//               {	
//               g_Tagdist[anc_id_recv]=   hex_dist2;
//               g_flag_Taggetdist[anc_id_recv]=0;
//					
//               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);
//         /* 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++;
//			
//            LED0_BLINK;
//            
//               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)
         {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
         { };
      }
         /* Clear TXFRS event. */
         dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除标志位
//               memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
////               if(hex_dist2!=0xffff)
////               {	
////               g_Tagdist[anc_id_recv]=   hex_dist2;
////               g_flag_Taggetdist[anc_id_recv]=0;
////					
////               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)
//         {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). */
//         /* Increment frame sequence number after transmission of the final message (modulo 256). */

         random_delay_tim = 0;
      }
      else
      {
         random_delay_tim = DFT_RAND_DLY_TIM_MS; //如果通讯失败，将间隔时间增加5ms，避开因为多标签同时发送引起的冲突。
      }
   }
   else
   {
      /* Clear RX error events in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
      random_delay_tim = DFT_RAND_DLY_TIM_MS;
   }
//   deca_sleep(10);
}
//   dwt_entersleep();
//         random_delay_tim = 0;
//      }
//      else
//      {
//         random_delay_tim = DFT_RAND_DLY_TIM_MS; //如果通讯失败，将间隔时间增加5ms，避开因为多标签同时发送引起的冲突。
//      }
//   }
//   else
//   {
//      /* Clear RX error events in the DW1000 status register. */
//      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
//      random_delay_tim = DFT_RAND_DLY_TIM_MS;
//   }
////   deca_sleep(10);
//}
////   dwt_entersleep();


   /* Execute a delay between ranging exchanges. */
      delay_ms(30);
//   /* 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];
//}
//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];


#define SINGLEPOLL_BASENUM 5
uint16_t singlepoll_baseid[20]={0x8166,0x9804,0x9689,0x9686,0x8279,0x9815,0x9803,0x9814,0x9687,0x9774,0x8278,0x9743,0x9688,0x9771,0x8277};
int32_t singlepoll_basedist[20];
//#define SINGLEPOLL_BASENUM 5
//uint16_t singlepoll_baseid[20]={0x8166,0x9804,0x9689,0x9686,0x8279,0x9815,0x9803,0x9814,0x9687,0x9774,0x8278,0x9743,0x9688,0x9771,0x8277};
//int32_t singlepoll_basedist[20];

extern u8 userkey_state;
extern float motor_keeptime;
uint8_t changemainbase_count=0,gotosleep_flag,singlepoll_i;
uint32_t frame_len;
int32_t salvebase_mindist;
int16_t intheight;
static uint8_t send_buffer[200];
void DiscPoll(void)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   u32 start_poll;
   int32_t minddist=0x1ffff,minid=-1,temp_dist;
   u8 i,j,getsync_flag=0;
    u16 mindist_ancid,random_delay_tim,tempval;
    
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   frame_seq_nb++;
   for(uint8_t singlepoll_i=0;singlepoll_i<10;singlepoll_i++)
   {
       
//extern float motor_keeptime;
//uint8_t changemainbase_count=0,gotosleep_flag,singlepoll_i;
//uint32_t frame_len;
//int32_t salvebase_mindist;
//int16_t intheight;
//static uint8_t send_buffer[200];
//void DiscPoll(void)
//{
//   uint32_t frame_len;
//   uint32_t final_tx_time;
//   u32 start_poll;
//   int32_t minddist=0x1ffff,minid=-1,temp_dist;
//   u8 i,j,getsync_flag=0;
//    u16 mindist_ancid,random_delay_tim,tempval;
//    
//    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
//    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
//   frame_seq_nb++;
//   for(uint8_t singlepoll_i=0;singlepoll_i<10;singlepoll_i++)
//   {
//       

   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
    nearbase_num = 0;
        
   // send_buffer[BATTARY_IDX] = bat_percent;
   //send_buffer[BUTTON_IDX] = !GET_USERKEY|stationary_flag<<1|gotosleep_flag<<2;
   send_buffer[SEQUENCE_IDX] = frame_seq_nb;
    send_buffer[SEQUENCEH_IDX] = (frame_seq_nb)>>8;
   send_buffer[NEARBASENUM_INDEX] = nearbase_num;
     
//   memcpy(&send_buffer[NEARBASEID_INDEX],&singlepoll_baseid,nearbase_num*2);
//    memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*2],&singlepoll_basedist,nearbase_num*4+4);
//    intheight+=g_com_map[HEIGHTOFFEST_INDEX];
 //   memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*6],&intheight,2);
   send_buffer[MESSAGE_TYPE_IDX] = DISCOVERPOLL;	
//   singlepoll_basedist[singlepoll_i]= 0x1ffff;
   send_buffer[NEARP_TAGFREQ_INDEX] = tag_frequency;	
   send_buffer[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;	
    send_buffer[ANCHOR_ID_IDX] = singlepoll_i;
   memcpy(&send_buffer[TAG_ID_IDX],&dev_id,2);	
   dwt_writetxdata(28+6*nearbase_num, send_buffer, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(28+6*nearbase_num, 0);//设置超宽带发送数据长度
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
//   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
//    nearbase_num = 0;
//        
//   // send_buffer[BATTARY_IDX] = bat_percent;
//   //send_buffer[BUTTON_IDX] = !GET_USERKEY|stationary_flag<<1|gotosleep_flag<<2;
//   send_buffer[SEQUENCE_IDX] = frame_seq_nb;
//    send_buffer[SEQUENCEH_IDX] = (frame_seq_nb)>>8;
//   send_buffer[NEARBASENUM_INDEX] = nearbase_num;
//     
////   memcpy(&send_buffer[NEARBASEID_INDEX],&singlepoll_baseid,nearbase_num*2);
////    memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*2],&singlepoll_basedist,nearbase_num*4+4);
////    intheight+=g_com_map[HEIGHTOFFEST_INDEX];
// //   memcpy(&send_buffer[NEARBASEID_INDEX+nearbase_num*6],&intheight,2);
//   send_buffer[MESSAGE_TYPE_IDX] = DISCOVERPOLL;	
////   singlepoll_basedist[singlepoll_i]= 0x1ffff;
//   send_buffer[NEARP_TAGFREQ_INDEX] = tag_frequency;	
//   send_buffer[NEARP_TAGSLOTPOS_INDEX] = tagslotpos;	
//    send_buffer[ANCHOR_ID_IDX] = singlepoll_i;
//   memcpy(&send_buffer[TAG_ID_IDX],&dev_id,2);	
//   dwt_writetxdata(28+6*nearbase_num, send_buffer, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
//   dwt_writetxfctrl(28+6*nearbase_num, 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;}
                }

                //offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
//                SetNextPollTime(tagslotpos);
      }
         memcpy(&send_buffer[NEARBASEID_INDEX+singlepoll_i*2],&rx_buffer[ANCHOR_ID_IDX],2);
         memcpy(&temp_dist, &rx_buffer[DIST_IDX], 4);
         memcpy(&send_buffer[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
           // singlepoll_basedist[singlepoll_i] = temp_dist;
       // memcpy(&singlepoll_baseid[singlepoll_i], &rx_buffer[ANCHOR_ID_IDX], 2);
            
            if(temp_dist<minddist)
            {
                minddist = temp_dist;
                memcpy(&mindist_ancid,&rx_buffer[ANCHOR_ID_IDX],2);
                
            }
            send_buffer[MESSAGE_TYPE_IDX] = DISCOVERFINAL;	
         /* Compute final message transmission time. See NOTE 9 below. */
         final_tx_time = (resp_rx_ts + (600 * 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(&send_buffer[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
         final_msg_set_ts(&send_buffer[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
         final_msg_set_ts(&send_buffer[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
         dwt_writetxdata(25,send_buffer, 0);//将发送数据写入DW1000
         dwt_writetxfctrl(25, 0);//设定发送数据长度
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送			
			
 
         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
      {
           //  singlepoll_basedist[singlepoll_i] = 0x1ffff;
         random_delay_tim = DFT_RAND_DLY_TIM_MS; //如果通讯失败，将间隔时间增加5ms，避开因为多标签同时发送引起的冲突。
      }
   }
   else
   {
     //   singlepoll_basedist[singlepoll_i] = 0x1ffff;
   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
      random_delay_tim = DFT_RAND_DLY_TIM_MS;
   }
   // delay_ms(1);
}
    if(minddist!=0x1ffff&&minddist!=0)
    {
         trygetnearmsg_times = 0;
            tag_state = GETNEARMSG;            
            mainbase_id = mindist_ancid;   
            GetNearMsg();
    }
   
   if(getsync_flag==0)
   {
//   tagslotpos--;
//         if(tagslotpos==0||tagslotpos>max_slotpos)
//      {		
//         tagslotpos=max_slotpos;	
//      }
//      tyncpoll_time=(tagslotpos-1)*slottime;
   }
//   mainbase_id=minid;
//   mainbase_id = 0x4;
}
int16_t target_offsettime = 850;
//#define TAGET_OFFSETTIME 940
#define MAX_NEARBASE_ANCNUM 11
u16 rec_maxrangelen,rec_anc_signalpower[MAX_NEARBASE_ANCNUM];
u8 outrange_times;
extern u32 synctimer;
u32 target_time=100;
u8 pd_i;
u32 temptime,delaytime,expect_anctime;
int32_t error_time;
u16 delaycount;
int16_t poll_offsettime=-6900,clockoffset;
extern u16 slotpos_intoatl, slotpos;
void NextPollDelay(u32 anchor_time)
{
      //tagslotpos = 0;
      error_time = anchor_time-expect_anctime;
   if(error_time<2000&&error_time>0)
      clockoffset += (target_offsettime-error_time)*0.1;
    for(pd_i=0;pd_i<tag_frequency;pd_i++)
    {
        slotpos_intoatl = pd_i*bigslot_num+tagslotpos;
        temptime = (slotpos_intoatl*SLOTTIME_MS+100)*100;
        if(anchor_time<temptime-5000)
        { 
               current_slotnum = pd_i;
            delaytime = temptime-anchor_time+poll_offsettime+clockoffset;  
                  expect_anctime = temptime;
            return ;
        }
    }
    temptime = (tagslotpos*SLOTTIME_MS+100+10000)*100;
      expect_anctime = temptime-1000000;
    delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
   return ;
}
u16 lpcount,poll_startcount,last_lpcount;
u16 waketopolltimeus = 3200;
#define BASENUM_COMTIME 244
u32 last_anchor_time;
void SetLPTimer(u32 anchor_time)
{
    NextPollDelay(anchor_time);
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    lpcount = current_count+delaytime/LPTIMER_LSB+(10-nearbase_num)*8;
      poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
      if(poll_startcount>=32768)
      {
      poll_startcount -=32768;
      }
      if(lpcount>=32768)
      {
      lpcount -=32768;
      }
    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
      last_lpcount = lpcount;
   }

void NextSlotDelayMs(int16_t delayms)
{
   current_slotnum++;
   if(current_slotnum>=tag_frequency)
   {	
      current_slotnum = 0;		
      lpcount = last_lpcount+31457/tag_frequency+1311+32.767*delayms;//(float)(delayms+40)*32.767 ;//(200/tag_frequency+delayms)*4.8*1000/LPTIMER_LSB
   }else{
      lpcount = last_lpcount+31457/tag_frequency+32.767*delayms;//(float)(200/tag_frequency*48+delayms*10)*3.3 ;
   }
            if(lpcount>=32768)
            {
            lpcount -=32768;
            }		
            poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
      if(poll_startcount>=32768)
      {
      poll_startcount -=32768;
      }
         __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
            last_lpcount = lpcount;
}

u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;
void GetNearMsg(void)
{
   u32 start_poll,frame_len;
    u8 nearmsg_i=0;
   NextSlotDelayMs(0);
    for(nearmsg_i=0;nearmsg_i<MAX_NEARBASE_NUM;nearmsg_i++)
    {
        nearbase_distlist[nearmsg_i] = 0x1ffff;
    }
//mainbase_id = 0x1;
   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] = POS_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);//设置超宽带发送数据长度
      current_count=HAL_LPTIM_ReadCounter(&hlptim1);
      while(current_count<poll_startcount||current_count>poll_startcount+16384)
      {
         current_count=HAL_LPTIM_ReadCounter(&hlptim1);
         if(current_count<poll_startcount-300)
         {
            break;
         }
      }
   dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED);
      NextSlotDelayMs(0);		
   /* 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] == POS_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);
         memcpy(nearbaseid_list2,&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;
   }
}
void SendHuiZongData(void)
{
                usart_send[2] = 0x0c;//正常模式 
               usart_send[3] = 15+8*(nearbase_num+1);//数据段长度
               memcpy(&usart_send[4],&dev_id,2);
                    usart_send[6] = frame_seq_nb;
                    usart_send[7] = frame_seq_nb>>8;
               usart_send[8] = bat_percent;
                    usart_send[9] = tx_near_msg[BUTTON_IDX];

                    memcpy(&usart_send[10],&intheight,2);
               usart_send[12] = tag_frequency;
                    usart_send[13] = tagslotpos;
                    usart_send[14] = 0;
                    usart_send[15] = 0;
               usart_send[16] = nearbase_num+1;
                    memcpy(&usart_send[17],&mainbase_id,2);
                    memcpy(&usart_send[19],&nearbaseid_list,2*nearbase_num);
                    memcpy(&usart_send[19+nearbase_num*2],&nearbase_distlist,4*nearbase_num+4);
                memcpy(&usart_send[19+nearbase_num*6+4],&rec_anc_signalpower,2*nearbase_num+2);
                    
               checksum = Checksum_u16(&usart_send[2],23+8*nearbase_num);
               memcpy(&usart_send[25+8*nearbase_num],&checksum,2);
               USART_puts(usart_send,27+8*nearbase_num);
               //HexToAsciiSendUDP(usart_send,27+8*nearbase_num);
}
#define FREQ_OFFSET_MULTIPLIER          (998.4e6/2.0/1024.0/131072.0)
#define FREQ_OFFSET_MULTIPLIER_110KB    (998.4e6/2.0/8192.0/131072.0)

// Multiplication factors to convert frequency offset in Hertz to PPM crystal offset
// NB: also changes sign so a positive value means the local RX clock is running slower than the remote TX device.

#define HERTZ_TO_PPM_MULTIPLIER_CHAN_1     (-1.0e6/3494.4e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_2     (-1.0e6/3993.6e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_3     (-1.0e6/4492.8e6)
#define HERTZ_TO_PPM_MULTIPLIER_CHAN_5     (-1.0e6/6489.6e6)
int32_t test2;
uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;
uint8_t motor_flag;
uint32_t testtimer[10],testtimer2,anc_pollrx[11],anc_resptx[11],tag_resprx[11];
int32_t anc_clockoffset[11];
int16_t anc_distoffset[11];
extern uint8_t Pah_HRD_flag_fangchai;
uint8_t get_newdist,notenoughdist_count;
void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)
{
    memcpy(&anc_pollrx[i],pollrx,4);
    memcpy(&anc_resptx[i],resptx,4);
    memcpy(&tag_resprx[i],&resprx,4);
    memcpy(&anc_distoffset[i],distoffset,2);
    anc_clockoffset[i] = test2;
}
float clockOffsetRatio;
double rtd_init, rtd_resp;
double tof,distance;
extern int32_t dwt_readcarrierintegrator(void) ;
void CalculateDists(void)
{
    for(int i=0; i<11; i++)
    {
       // rec_anc_signalpower[i] = exsistbase_list[i];
        if(exsistbase_list[i]>0)
        {

            exsistbase_list[i]--;
#ifdef _UWB_4G
            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
#else
            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
#endif
            rtd_init = tag_resprx[i] - poll_tx_ts&0xffffffff;
            rtd_resp = anc_resptx[i] - anc_pollrx[i];
            tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
            distance = tof * SPEED_OF_LIGHT;
            if(distance>-10&&distance<1000)
            {
                nearbase_distlist[i] = distance*100+anc_distoffset[i];
            } else {
                nearbase_distlist[i] = 0x1ffff;
            }
        } else {
            nearbase_distlist[i] = 0x1ffff;
        }

    }
}
u32 last_mindist_slavebaseid;
void NearPoll(void)
{
	
   uint32_t temp1,temp2,dw_systime,mindist_slavebaseid;
    static u8 notenoughdist_count=0;
   uint32_t final_tx_time;
   u32 start_poll;
   u8 i,j,getsync_flag=0,timeout,get_newdist;
   // USART_putc(current_slotnum);
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
   tag_succ_times = 0;
   salvebase_mindist=999999;
    get_newdist = 0;
   if(next_nearbase_num>=MAX_NEARBASE_NUM)
   {
      next_nearbase_num = MAX_NEARBASE_NUM-1;
   }
   if(nearbase_num>10)
   {nearbase_num = 10;}
//   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);

   recbase_num=0;
//   motor_state=0;
    if(motor_state!=0&&motor_state!=3)
    {
        motor_flag = 1;
    }else{
        motor_flag = 0;
    }
	
   tx_near_msg[BATTARY_IDX] = bat_percent;
   tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5;
   tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
    tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
   tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
   memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);

    
    if(intheight!=0)
    intheight+=g_com_map[HEIGHTOFFEST_INDEX];
    
    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
   tx_near_msg[MESSAGE_TYPE_IDX] = POS_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(29+8*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
   dwt_writetxfctrl(29+8*nearbase_num, 0);//设置超宽带发送数据长度
            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
		
      while(current_count<poll_startcount||current_count>poll_startcount+16384)
      {
         current_count=HAL_LPTIM_ReadCounter(&hlptim1);
         if(current_count<poll_startcount-300)
         {
            break;
         }
      }
   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
//   HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);
//    for(i=0;i<nearbase_num+1;i++)
//    {
//        nearbase_distlist[i] = 0x1ffff;
//    }
    
   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)+3;
   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);//清楚寄存器标志位
                     frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;   //获得接收到的数据长度
                     dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
                            test2 = dwt_readcarrierintegrator();
                     dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
                     dwt_rxenable(0);//打开接收
                     //HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
                     if (rx_buffer[MESSAGE_TYPE_IDX] == POS_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)
                        {
                                     exsistbase_list[0]=KEEP_TIMES;
                                   SetANCTimestap(0,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);

                           //////////////////////////////////时间同步
                           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]=POS_FINAL;
                           rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>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);
                                        }
//                           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);
                                    temp_dist = nearbase_distlist[0];
                                    if(temp_dist!=nearbase_distlist[0])
                                    {
                                        get_newdist++;
                                    }
                           mainbase_dist=temp_dist;
                                    nearbase_distlist[0] = temp_dist;
                                    base_mindist = temp_dist;
//                           if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
//                              motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;
                                    if(frame_len==38)
                                     {
                                         memcpy(&rec_maxrangelen,&rx_buffer[MAXRANGE_DISTANCE],2);
                                         memcpy(&rec_anc_signalpower[0],&rx_buffer[ANC_SIGNALPOWER],2);
                                         if(mainbase_dist>rec_maxrangelen&&mainbase_dist>0&&rec_maxrangelen!=0)
                                         {
                                             if(outrange_times++>OUTRANGE_RESTARTTIMES)
                                             {
                                                tag_state = STARTPOLL;
                                             }
                                         }else{
                                             outrange_times = 0;
                                         }
                                     }
                                 
									
									
                           mainbase_lost_count=0;
                           flag_finalsend=1;
//                           memcpy(&mainbase_dist,&rx_buffer[DIST_IDX],4);
		

                           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);	
                           SetLPTimer(ancsync_time);
//                           if(result==0)
//                                 {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
//                                 { };}			
//HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);											
                              break;
                        }else{
									
                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);	
                                    SetANCTimestap(rec_nearbasepos+1,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);
									
                           exsistbase_list[rec_nearbasepos+1]=KEEP_TIMES;
                           //memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
                                    temp_dist = nearbase_distlist[rec_nearbasepos+1];
                                    if(frame_len==38)
                                    memcpy(&rec_anc_signalpower[rec_nearbasepos+1],&rx_buffer[ANC_SIGNALPOWER],2);
                           memcpy(&nearbase_switchdistlist[rec_nearbasepos],&rx_buffer[NR_NEARSWITCH_DISTANCE],2);
                                    if(temp_dist!=nearbase_distlist[rec_nearbasepos+1])
                                    {
                                        get_newdist++;
                                    }
                           nearbase_distlist[rec_nearbasepos+1]=temp_dist;   //   nearbase_distlist[1]对应   rec_nearbaseid[0]的距离					
                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
                              if(temp_dist<salvebase_mindist&&nearbase_switchdistlist[rec_nearbasepos]!=0&&(nearbase_switchdistlist[rec_nearbasepos]==1||temp_dist<nearbase_switchdistlist[rec_nearbasepos]))
                           {
                              salvebase_mindist = temp_dist;
                              mindist_slavebaseid = rec_nearbaseid;
                           }
                           #ifdef SWITCHBASE_ZHUANDIAN
//                                       if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]>1&&temp_dist>0)
//                             {
//                                            mainbase_id = rec_nearbaseid;
//                                            tag_state = GETNEARMSG;
//                                 trygetnearmsg_times = 0;
//                                        }
                                    #endif

                  }								
                     }
         //            HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);
                  }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);
         }
            if(flag_finalsend!=1)
            {flag_finalsend = 2;}
         dwt_forcetrxoff();
         dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
//         HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
         if(mainbase_lost_count==0)
         {
            #ifdef SWITCHBASE_DIST
            if(salvebase_mindist<mainbase_dist- THRESHOLD_CHANGE_MAINBASE_DIST )
            {
               if(mindist_slavebaseid==last_mindist_slavebaseid)
               {
                  changemainbase_count++;
               if(changemainbase_count>2)
               {
                        changemainbase_count = 0;
                        mainbase_id = mindist_slavebaseid;
                        tag_state = GETNEARMSG;                    
                        trygetnearmsg_times = 0;
                        GetNearMsg();
               }}
               else{
                  changemainbase_count = 1;
                  last_mindist_slavebaseid = mindist_slavebaseid;
               }
            }else{
               changemainbase_count = 0;
            }
         #endif
         }else if(mainbase_lost_count>10)
         {
                mainbase_lost_count = 0;
            tag_state = DISCPOLL;
                DiscPoll();
         }	
         if(mainbase_lost_count!=0)
         {
            if(mainbase_lost_count<=5)
            {NextSlotDelayMs(0);
            }else{
               NextSlotDelayMs(0);
            }
         }	
CalculateDists();
         if(para_update)
         {
			
            pack_msgtype = rec_remotepara[0];
            pack_index = rec_remotepara[1];
            pack_length = rec_remotepara[2];
            if(pack_msgtype==2)
            {
               if( pack_index == MOTOR_ONTIME_INDEX)
               {
                        if(motor_keeptime==0)
                            motor_keeptime = rec_remotepara[3];
               }else if( pack_index == 2*COM_INTERVAL)
                    {
                        if(memcmp(&g_com_map[COM_INTERVAL],&rec_remotepara[3],2)!=0)
                        {
                            memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
                            save_com_map_to_flash();
                            tag_frequency = 1000/g_com_map[COM_INTERVAL];
                            bigslot_num = TOTAL_SLOTNUM/tag_frequency;
                        }
                    }
                    else{
                        if(pack_index<200)
                        {
                            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();
                        }
               }
            }
         }
//            if(get_newdist>=2)
//            {
//                notenoughdist_count = 0;
//            }else{
//                if(notenoughdist_count++>10)
//   
//   /* 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)
//                {
//                    notenoughdist_count = 0;
//                    tag_state = DISCPOLL;
//                    tmp_time-=999;
//                    sync_timer++;
//                    if(sync_timer>=1010)
//                        {sync_timer=0;}
//                }

//                //offsettimeus=ancsync_time-current_count*LPTIMER_LSB+offset;
////                SetNextPollTime(tagslotpos);
//      }
//         memcpy(&send_buffer[NEARBASEID_INDEX+singlepoll_i*2],&rx_buffer[ANCHOR_ID_IDX],2);
//         memcpy(&temp_dist, &rx_buffer[DIST_IDX], 4);
//         memcpy(&send_buffer[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
//           // singlepoll_basedist[singlepoll_i] = temp_dist;
//       // memcpy(&singlepoll_baseid[singlepoll_i], &rx_buffer[ANCHOR_ID_IDX], 2);
//            
//            if(temp_dist<minddist)
//            {
//                minddist = temp_dist;
//                memcpy(&mindist_ancid,&rx_buffer[ANCHOR_ID_IDX],2);
//                
//            }
         for(i=0;i<nearbase_num;i++)
         {
                if(nearbaseid_list[i]!=nearbaseid_list2[i])
                {
                    tag_state = GETNEARMSG;
                    GetNearMsg();
                           trygetnearmsg_times = 0;
                           nearbaseid_list0[i]=1;
                }
         }
        //    SendHuiZongData();
   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);
//            send_buffer[MESSAGE_TYPE_IDX] = DISCOVERFINAL;	
//         /* Compute final message transmission time. See NOTE 9 below. */
//         final_tx_time = (resp_rx_ts + (600 * 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(&send_buffer[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);//将T1，T4，T5写入发送数据
//         final_msg_set_ts(&send_buffer[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
//         final_msg_set_ts(&send_buffer[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
//         dwt_writetxdata(25,send_buffer, 0);//将发送数据写入DW1000
//         dwt_writetxfctrl(25, 0);//设定发送数据长度
//         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送			
//			
// 
//         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
//      {
//           //  singlepoll_basedist[singlepoll_i] = 0x1ffff;
//         random_delay_tim = DFT_RAND_DLY_TIM_MS; //如果通讯失败，将间隔时间增加5ms，避开因为多标签同时发送引起的冲突。
//      }
//   }
//   else
//   {
//     //   singlepoll_basedist[singlepoll_i] = 0x1ffff;
//   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
//      random_delay_tim = DFT_RAND_DLY_TIM_MS;
//   }
//   // delay_ms(1);
//}
//    if(minddist!=0x1ffff&&minddist!=0)
//    {
//         trygetnearmsg_times = 0;
//            tag_state = GETNEARMSG;            
//            mainbase_id = mindist_ancid;   
//            GetNearMsg();
//    }
//   
//   if(getsync_flag==0)
//   {
////   tagslotpos--;
////         if(tagslotpos==0||tagslotpos>max_slotpos)
////      {		
////         tagslotpos=max_slotpos;	
////      }
////      tyncpoll_time=(tagslotpos-1)*slottime;
//   }
////   mainbase_id=minid;
////   mainbase_id = 0x4;
//}
//int16_t target_offsettime = 850;
////#define TAGET_OFFSETTIME 940
//#define MAX_NEARBASE_ANCNUM 11
//u16 rec_maxrangelen,rec_anc_signalpower[MAX_NEARBASE_ANCNUM];
//u8 outrange_times;
//extern u32 synctimer;
//u32 target_time=100;
//u8 pd_i;
//u32 temptime,delaytime,expect_anctime;
//int32_t error_time;
//u16 delaycount;
//int16_t poll_offsettime=-6900,clockoffset;
//extern u16 slotpos_intoatl, slotpos;
//void NextPollDelay(u32 anchor_time)
//{
//      //tagslotpos = 0;
//      error_time = anchor_time-expect_anctime;
//   if(error_time<2000&&error_time>0)
//      clockoffset += (target_offsettime-error_time)*0.1;
//    for(pd_i=0;pd_i<tag_frequency;pd_i++)
//    {
//        slotpos_intoatl = pd_i*bigslot_num+tagslotpos;
//        temptime = (slotpos_intoatl*SLOTTIME_MS+100)*100;
//        if(anchor_time<temptime-5000)
//        { 
//               current_slotnum = pd_i;
//            delaytime = temptime-anchor_time+poll_offsettime+clockoffset;  
//                  expect_anctime = temptime;
//            return ;
//        }
//    }
//    temptime = (tagslotpos*SLOTTIME_MS+100+10000)*100;
//      expect_anctime = temptime-1000000;
//    delaytime = temptime-anchor_time+poll_offsettime+clockoffset;
//   return ;
//}
//u16 lpcount,poll_startcount,last_lpcount;
//u16 waketopolltimeus = 3200;
//#define BASENUM_COMTIME 244
//u32 last_anchor_time;
//void SetLPTimer(u32 anchor_time)
//{
//    NextPollDelay(anchor_time);
//    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//    lpcount = current_count+delaytime/LPTIMER_LSB+(10-nearbase_num)*8;
//      poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
//      if(poll_startcount>=32768)
//      {
//      poll_startcount -=32768;
//      }
//      if(lpcount>=32768)
//      {
//      lpcount -=32768;
//      }
//    __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
//      last_lpcount = lpcount;
//   }

}
extern uint8_t module_power,imu_enable,motor_enable;
void Registor_Poll(void)
{
   static u8 regpoll_count=0;
   mainbase_lost_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);
      NextSlotDelayMs(-2);
		
      dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
//void NextSlotDelayMs(int16_t delayms)
//{
//   current_slotnum++;
//   if(current_slotnum>=tag_frequency)
//   {	
//      current_slotnum = 0;		
//      lpcount = last_lpcount+31457/tag_frequency+1311+32.767*delayms;//(float)(delayms+40)*32.767 ;//(200/tag_frequency+delayms)*4.8*1000/LPTIMER_LSB
//   }else{
//      lpcount = last_lpcount+31457/tag_frequency+32.767*delayms;//(float)(200/tag_frequency*48+delayms*10)*3.3 ;
//   }
//            if(lpcount>=32768)
//            {
//            lpcount -=32768;
//            }		
//            poll_startcount = lpcount+(waketopolltimeus)/LPTIMER_LSB;
//      if(poll_startcount>=32768)
//      {
//      poll_startcount -=32768;
//      }
//         __HAL_LPTIM_COMPARE_SET(&hlptim1, lpcount);
//            last_lpcount = lpcount;
//}

      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);
        send_buffer[REGP_BATTARY_INDEX] = bat_percent;
//    memcpy(&tx_near_msg[REGP_HEIGHTOFFSET_INDEX],&g_com_map[HEIGHTOFFEST_INDEX],2);
      dwt_writetxdata(23, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
      dwt_writetxfctrl(23, 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);																			
                           SetLPTimer(ancsync_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);
                           memcpy(nearbaseid_list2,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
                           if(tagslotpos!=255)
                                        tag_state = DISCPOLL;
                        }
                     }
}
//u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32],mainbase_type;;
//void GetNearMsg(void)
//{
//   u32 start_poll,frame_len;
//    u8 nearmsg_i=0;
//   NextSlotDelayMs(0);
//    for(nearmsg_i=0;nearmsg_i<MAX_NEARBASE_NUM;nearmsg_i++)
//    {
//        nearbase_distlist[nearmsg_i] = 0x1ffff;
//    }
////mainbase_id = 0x1;
//   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] = POS_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);//设置超宽带发送数据长度
//      current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//      while(current_count<poll_startcount||current_count>poll_startcount+16384)
//      {
//         current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//         if(current_count<poll_startcount-300)
//         {
//            break;
//         }
//      }
//   dwt_starttx(DWT_START_TX_IMMEDIATE| DWT_RESPONSE_EXPECTED);
//      NextSlotDelayMs(0);		
//   /* 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] == POS_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);
//         memcpy(nearbaseid_list2,&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;
//   }
//}
//void SendHuiZongData(void)
//{
//                usart_send[2] = 0x0c;//正常模式 
//               usart_send[3] = 15+8*(nearbase_num+1);//数据段长度
//               memcpy(&usart_send[4],&dev_id,2);
//                    usart_send[6] = frame_seq_nb;
//                    usart_send[7] = frame_seq_nb>>8;
//               usart_send[8] = bat_percent;
//                    usart_send[9] = tx_near_msg[BUTTON_IDX];

//                    memcpy(&usart_send[10],&intheight,2);
//               usart_send[12] = tag_frequency;
//                    usart_send[13] = tagslotpos;
//                    usart_send[14] = 0;
//                    usart_send[15] = 0;
//               usart_send[16] = nearbase_num+1;
//                    memcpy(&usart_send[17],&mainbase_id,2);
//                    memcpy(&usart_send[19],&nearbaseid_list,2*nearbase_num);
//                    memcpy(&usart_send[19+nearbase_num*2],&nearbase_distlist,4*nearbase_num+4);
//                memcpy(&usart_send[19+nearbase_num*6+4],&rec_anc_signalpower,2*nearbase_num+2);
//                    
//               checksum = Checksum_u16(&usart_send[2],23+8*nearbase_num);
//               memcpy(&usart_send[25+8*nearbase_num],&checksum,2);
//               USART_puts(usart_send,27+8*nearbase_num);
//               //HexToAsciiSendUDP(usart_send,27+8*nearbase_num);
//}
//#define FREQ_OFFSET_MULTIPLIER          (998.4e6/2.0/1024.0/131072.0)
//#define FREQ_OFFSET_MULTIPLIER_110KB    (998.4e6/2.0/8192.0/131072.0)

//// Multiplication factors to convert frequency offset in Hertz to PPM crystal offset
//// NB: also changes sign so a positive value means the local RX clock is running slower than the remote TX device.

//#define HERTZ_TO_PPM_MULTIPLIER_CHAN_1     (-1.0e6/3494.4e6)
//#define HERTZ_TO_PPM_MULTIPLIER_CHAN_2     (-1.0e6/3993.6e6)
//#define HERTZ_TO_PPM_MULTIPLIER_CHAN_3     (-1.0e6/4492.8e6)
//#define HERTZ_TO_PPM_MULTIPLIER_CHAN_5     (-1.0e6/6489.6e6)
//int32_t test2;
//uint8_t pack_length = 0,pack_index = 0,pack_msgtype = 0;
//uint8_t motor_flag;
//uint32_t testtimer[10],testtimer2,anc_pollrx[11],anc_resptx[11],tag_resprx[11];
//int32_t anc_clockoffset[11];
//int16_t anc_distoffset[11];
//extern uint8_t Pah_HRD_flag_fangchai;
//uint8_t get_newdist,notenoughdist_count;
//void SetANCTimestap(uint8_t i,uint8_t* pollrx,uint8_t* resptx,uint32_t resprx,uint8_t* distoffset)
//{
//    memcpy(&anc_pollrx[i],pollrx,4);
//    memcpy(&anc_resptx[i],resptx,4);
//    memcpy(&tag_resprx[i],&resprx,4);
//    memcpy(&anc_distoffset[i],distoffset,2);
//    anc_clockoffset[i] = test2;
//}
//float clockOffsetRatio;
//double rtd_init, rtd_resp;
//double tof,distance;
//extern int32_t dwt_readcarrierintegrator(void) ;
//void CalculateDists(void)
//{
//    for(int i=0; i<11; i++)
//    {
//       // rec_anc_signalpower[i] = exsistbase_list[i];
//        if(exsistbase_list[i]>0)
//        {

//            exsistbase_list[i]--;
//#ifdef _UWB_4G
//            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
//#else
//            clockOffsetRatio = anc_clockoffset[i] * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_5 / 1.0e6) ;
//#endif
//            rtd_init = tag_resprx[i] - poll_tx_ts&0xffffffff;
//            rtd_resp = anc_resptx[i] - anc_pollrx[i];
//            tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
//            distance = tof * SPEED_OF_LIGHT;
//            if(distance>-10&&distance<1000)
//            {
//                nearbase_distlist[i] = distance*100+anc_distoffset[i];
//            } else {
//                nearbase_distlist[i] = 0x1ffff;
//            }
//        } else {
//            nearbase_distlist[i] = 0x1ffff;
//        }

//    }
//}
//u32 last_mindist_slavebaseid;
//void NearPoll(void)
//{
//	
//   uint32_t temp1,temp2,dw_systime,mindist_slavebaseid;
//    static u8 notenoughdist_count=0;
//   uint32_t final_tx_time;
//   u32 start_poll;
//   u8 i,j,getsync_flag=0,timeout,get_newdist;
//   // USART_putc(current_slotnum);
//    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
//    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);		
//   tag_succ_times = 0;
//   salvebase_mindist=999999;
//    get_newdist = 0;
//   if(next_nearbase_num>=MAX_NEARBASE_NUM)
//   {
//      next_nearbase_num = MAX_NEARBASE_NUM-1;
//   }
//   if(nearbase_num>10)
//   {nearbase_num = 10;}
////   HAL_GPIO_WritePin(LED0_GPIO, GPIO_PIN_9, GPIO_PIN_SET);

//   recbase_num=0;
////   motor_state=0;
//    if(motor_state!=0&&motor_state!=3)
//    {
//        motor_flag = 1;
//    }else{
//        motor_flag = 0;
//    }
//	
//   tx_near_msg[BATTARY_IDX] = bat_percent;
//   tx_near_msg[BUTTON_IDX] = userkey_state|stationary_flag<<1|gotosleep_flag<<2|motor_flag<<5;
//   tx_near_msg[SEQUENCE_IDX] = frame_seq_nb;
//    tx_near_msg[SEQUENCEH_IDX] = (frame_seq_nb++)>>8;
//   tx_near_msg[NEARBASENUM_INDEX] = nearbase_num;
//   memcpy(&tx_near_msg[NEARBASEID_INDEX],&nearbaseid_list,nearbase_num*2);
//    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*2],&nearbase_distlist,nearbase_num*4+4);
//    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*6+4],&rec_anc_signalpower,nearbase_num*2+2);

//    
//    if(intheight!=0)
//    intheight+=g_com_map[HEIGHTOFFEST_INDEX];
//    
//    memcpy(&tx_near_msg[NEARBASEID_INDEX+nearbase_num*8+6],&intheight,2);
//   tx_near_msg[MESSAGE_TYPE_IDX] = POS_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(29+8*nearbase_num, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
//   dwt_writetxfctrl(29+8*nearbase_num, 0);//设置超宽带发送数据长度
//            current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//		
//      while(current_count<poll_startcount||current_count>poll_startcount+16384)
//      {
//         current_count=HAL_LPTIM_ReadCounter(&hlptim1);
//         if(current_count<poll_startcount-300)
//         {
//            break;
//         }
//      }
//   dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//开启发送，发送完成后等待一段时间开启接收，等待时间在dwt_setrxaftertxdelay中设置
////   HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);
////    for(i=0;i<nearbase_num+1;i++)
////    {
////        nearbase_distlist[i] = 0x1ffff;
////    }
//    
//   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)+3;
//   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);//清楚寄存器标志位
//                     frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;   //获得接收到的数据长度
//                     dwt_readrxdata(rx_buffer, frame_len, 0);   //读取接收数据
//                            test2 = dwt_readcarrierintegrator();
//                     dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
//                     dwt_rxenable(0);//打开接收
//                     //HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
//                     if (rx_buffer[MESSAGE_TYPE_IDX] == POS_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)
//                        {
//                                     exsistbase_list[0]=KEEP_TIMES;
//                                   SetANCTimestap(0,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);

//                           //////////////////////////////////时间同步
//                           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]=POS_FINAL;
//                           rec_remotepara_state=rx_buffer[MOTORSTATE_INDEX]>>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);
//                                        }
////                           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);
//                                    temp_dist = nearbase_distlist[0];
//                                    if(temp_dist!=nearbase_distlist[0])
//                                    {
//                                        get_newdist++;
//                                    }
//                           mainbase_dist=temp_dist;
//                                    nearbase_distlist[0] = temp_dist;
//                                    base_mindist = temp_dist;
////                           if((rx_buffer[MOTORSTATE_INDEX]&0xf)!=3)
////                              motor_state=rx_buffer[MOTORSTATE_INDEX]&0xf;
//                                    if(frame_len==38)
//                                     {
//                                         memcpy(&rec_maxrangelen,&rx_buffer[MAXRANGE_DISTANCE],2);
//                                         memcpy(&rec_anc_signalpower[0],&rx_buffer[ANC_SIGNALPOWER],2);
//                                         if(mainbase_dist>rec_maxrangelen&&mainbase_dist>0&&rec_maxrangelen!=0)
//                                         {
//                                             if(outrange_times++>OUTRANGE_RESTARTTIMES)
//                                             {
//                                                tag_state = STARTPOLL;
//                                             }
//                                         }else{
//                                             outrange_times = 0;
//                                         }
//                                     }
//                                 
//									
//									
//                           mainbase_lost_count=0;
//                           flag_finalsend=1;
////                           memcpy(&mainbase_dist,&rx_buffer[DIST_IDX],4);
//		

//                           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);	
//                           SetLPTimer(ancsync_time);
////                           if(result==0)
////                                 {while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
////                                 { };}			
////HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);											
//                              break;
//                        }else{
//									
//                           rec_nearbasepos=FindNearBasePos(rec_nearbaseid);	
//                                    SetANCTimestap(rec_nearbasepos+1,&rx_buffer[RESP_MSG_POLL_RX_TS_IDX],&rx_buffer[RESP_MSG_RESP_TX_TS_IDX],resp_rx_ts,&rx_buffer[RESP_MSG_ANC_DISTOFFSET]);
//									
//                           exsistbase_list[rec_nearbasepos+1]=KEEP_TIMES;
//                           //memcpy(&temp_dist,&rx_buffer[DIST_IDX],4);
//                                    temp_dist = nearbase_distlist[rec_nearbasepos+1];
//                                    if(frame_len==38)
//                                    memcpy(&rec_anc_signalpower[rec_nearbasepos+1],&rx_buffer[ANC_SIGNALPOWER],2);
//                           memcpy(&nearbase_switchdistlist[rec_nearbasepos],&rx_buffer[NR_NEARSWITCH_DISTANCE],2);
//                                    if(temp_dist!=nearbase_distlist[rec_nearbasepos+1])
//                                    {
//                                        get_newdist++;
//                                    }
//                           nearbase_distlist[rec_nearbasepos+1]=temp_dist;   //   nearbase_distlist[1]对应   rec_nearbaseid[0]的距离					
//                           final_msg_set_ts(&tx_near_msg[FINAL_MSG_RESP_RX_NEARBASE_IDX+(rec_nearbasepos)*4], resp_rx_ts);
//                              if(temp_dist<salvebase_mindist&&nearbase_switchdistlist[rec_nearbasepos]!=0&&(nearbase_switchdistlist[rec_nearbasepos]==1||temp_dist<nearbase_switchdistlist[rec_nearbasepos]))
//                           {
//                              salvebase_mindist = temp_dist;
//                              mindist_slavebaseid = rec_nearbaseid;
//                           }
//                           #ifdef SWITCHBASE_ZHUANDIAN
////                                       if(temp_dist<nearbase_switchdistlist[rec_nearbasepos]&&nearbase_switchdistlist[rec_nearbasepos]>1&&temp_dist>0)
////                             {
////                                            mainbase_id = rec_nearbaseid;
////                                            tag_state = GETNEARMSG;
////                                 trygetnearmsg_times = 0;
////                                        }
//                                    #endif

//                  }								
//                     }
//         //            HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_RESET);
//                  }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);
//         }
//            if(flag_finalsend!=1)
//            {flag_finalsend = 2;}
//         dwt_forcetrxoff();
//         dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
////         HAL_GPIO_WritePin(LED1_G_GPIO_Port, LED1_G_Pin, GPIO_PIN_SET);
//         if(mainbase_lost_count==0)
//         {
//            #ifdef SWITCHBASE_DIST
//            if(salvebase_mindist<mainbase_dist- THRESHOLD_CHANGE_MAINBASE_DIST )
//            {
//               if(mindist_slavebaseid==last_mindist_slavebaseid)
//               {
//                  changemainbase_count++;
//               if(changemainbase_count>2)
//               {
//                        changemainbase_count = 0;
//                        mainbase_id = mindist_slavebaseid;
//                        tag_state = GETNEARMSG;                    
//                        trygetnearmsg_times = 0;
//                        GetNearMsg();
//               }}
//               else{
//                  changemainbase_count = 1;
//                  last_mindist_slavebaseid = mindist_slavebaseid;
//               }
//            }else{
//               changemainbase_count = 0;
//            }
//         #endif
//         }else if(mainbase_lost_count>10)
//         {
//                mainbase_lost_count = 0;
//            tag_state = DISCPOLL;
//                DiscPoll();
//         }	
//         if(mainbase_lost_count!=0)
//         {
//            if(mainbase_lost_count<=5)
//            {NextSlotDelayMs(0);
//            }else{
//               NextSlotDelayMs(0);
//            }
//         }	
//CalculateDists();
//         if(para_update)
//         {
//			
//            pack_msgtype = rec_remotepara[0];
//            pack_index = rec_remotepara[1];
//            pack_length = rec_remotepara[2];
//            if(pack_msgtype==2)
//            {
//               if( pack_index == MOTOR_ONTIME_INDEX)
//               {
//                        if(motor_keeptime==0)
//                            motor_keeptime = rec_remotepara[3];
//               }else if( pack_index == 2*COM_INTERVAL)
//                    {
//                        if(memcmp(&g_com_map[COM_INTERVAL],&rec_remotepara[3],2)!=0)
//                        {
//                            memcpy((uint8_t*)&g_com_map + pack_index, &rec_remotepara[3], pack_length);
//                            save_com_map_to_flash();
//                            tag_frequency = 1000/g_com_map[COM_INTERVAL];
//                            bigslot_num = TOTAL_SLOTNUM/tag_frequency;
//                        }
//                    }
//                    else{
//                        if(pack_index<200)
//                        {
//                            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();
//                        }
//               }
//            }
//         }
////            if(get_newdist>=2)
////            {
////                notenoughdist_count = 0;
////            }else{
////                if(notenoughdist_count++>10)
////                {
////                    notenoughdist_count = 0;
////                    tag_state = DISCPOLL;
////                }
////            }
//         for(i=0;i<nearbase_num;i++)
//         {
//                if(nearbaseid_list[i]!=nearbaseid_list2[i])
//                {
//                    tag_state = GETNEARMSG;
//                    GetNearMsg();
//                           trygetnearmsg_times = 0;
//                           nearbaseid_list0[i]=1;
//                }
//         }
//        //    SendHuiZongData();
//   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;
//   mainbase_lost_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);
//      NextSlotDelayMs(-2);
//		
//      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);
//        send_buffer[REGP_BATTARY_INDEX] = bat_percent;
////    memcpy(&tx_near_msg[REGP_HEIGHTOFFSET_INDEX],&g_com_map[HEIGHTOFFEST_INDEX],2);
//      dwt_writetxdata(23, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
//      dwt_writetxfctrl(23, 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);																			
//                           SetLPTimer(ancsync_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);
//                           memcpy(nearbaseid_list2,&rx_buffer[NEARBASEID_INDEX],nearbase_num*2);
//                           if(tagslotpos!=255)
//                                        tag_state = DISCPOLL;
//                        }
//                     }
//}
u32 id,error_times=0;

extern float Height;
@@ -1311,37 +1408,7 @@
        }
    }
    error_times = 0;
   switch(tag_state)
   {
      case REGPOLL:
         LED_LG_ON;		
         Registor_Poll();			
         LED_LG_OFF;
         break;
      case DISCPOLL:
         LED_LG_ON;		
         DiscPoll();
         LED_LG_OFF;
         break;
      case GETNEARMSG:
         trygetnearmsg_times++;	
         LED_LG_ON;
            GetNearMsg();	 
      LED_LG_OFF;
      if(trygetnearmsg_times>5)
      {
         tag_state = STARTPOLL;
      }
         break;
      case NEARPOLL:	
         NearPoll();
         //GetPressAndHeight();
         //intheight = Height*100;
         break;
        case SINGLEPOLL:
          //  Poll();
            break;
   }
    SwitchTagState();
    FZNearPoll();
    dwt_forcetrxoff();
     userkey_state = !GET_USERKEY;