#include "dw_mbx_anc.h"
///////////////////////////////////Ãâ²¼Ïßʱ¼äͬ²½´úÂë

#define CONFIG_REMAIN_TIME      120

sync_state_enum sync_state = Sync_Lost;
int16_t intheight;
static uint8_t sync_interval_s,range_state = 0,config_state_count = CONFIG_REMAIN_TIME;
static uint16_t sync_listen_ms;
void MbxInit(void)
{
   // config_state_count = CONFIG_REMAIN_TIME;
}
void SyncStateSwitch(void) //ͬ²½Çл»Âß¼­£¬¿ª»úLOST״̬£¬ÊÕµ½Ðźźó½øÈëͬ²½×´Ì¬£¬ÔÚͬ²½×´Ì¬ÖУ¬¸ù¾ÝÊÕµ½µÄÍø¹ØÐÅÏ¢Åжϲâ¾à»¹ÊÇÐÝÃß ²â¾à ÅäÖÃ״̬
{
    switch(sync_state)
    {
        case Sync_Lost:
            sync_interval_s = 100; //100
            sync_listen_ms =1500;
            range_state = 1;
            break;
        case Sync_Range:
            sync_interval_s = 10; //10
            sync_listen_ms =15;
            range_state = 1;
            if(config_state_count>0)
            {   config_state_count--;
                sync_interval_s = 1;
            }
            break;
        case Sync_Sleep:
            sync_interval_s = 10;//10
            sync_listen_ms =15;
            range_state = 0;
            if(config_state_count>0)
            {   config_state_count--;
                sync_interval_s = 1;
            }

            break;   
    }
}
uint8_t GetRangeState(void)
{
//    range_state = 0;
    return range_state;
}
#define SYNCLOST_THRES   10



static uint16_t current_count,poll_startcount,start_count,end_count,poll_timeout;
 uint8_t sync_interval_count = 100,flag_sync_success,sync_lost_count;
void SetFlagSyncSuccess(uint8_t value)
{
    flag_sync_success = value;
}

static uint16_t sync_wg_id,rec_sync_count;
uint8_t lora_recbuffer[255],lora_recbuffer_len;
static uint16_t wg_sync_polltime_100us,uwbpoll_time_count;
int16_t synclock_zero_count;
uint16_t GetUwbPollCount(void)
{
    uwbpoll_time_count = synclock_zero_count+g_com_map[UWBPOLLTIME_MS_IDX]*33;
    if(uwbpoll_time_count>=32768)
        uwbpoll_time_count-=32768;
    return uwbpoll_time_count;
}
uint16_t GetWGRecSyncCount(void)
{
    return rec_sync_count;
}
void SyncClockProcess(void) //ͬ²½Ê±ÖÓº¯Êý»ñÈ¡Ï´ÎUWBpollµÄ¶¨Ê±Æ÷count¡£
{

}
#define HEATBEAT_UPDATE_TIME_S 600  //ÐÄÌø°üÒ»¸öСʱÉÏ´«Ò»´Î
static uint16_t current_count,target_count,wg_report_id;
uint16_t heartbeat_count = HEATBEAT_UPDATE_TIME_S;
static uint8_t heatbeat_buffer[100];

void HeartbeatPoll(uint16_t wangguanid)
{//ÐÄÌø°üÉÏ´« ID °æ±¾ºÅ С×é UWB¹¦ÂÊ LORA¹¦ÂÊ UWB·¢ËÍʱ¼ä 
    heartbeat_count+=sync_interval_s;
    if(heartbeat_count>HEATBEAT_UPDATE_TIME_S)
    {     
        uint16_t checksum1 = 0;
        heartbeat_count = 0;
        heatbeat_buffer[MSG_TYPE_IDX] = LORA_MSGTYPE_ANC_HEARTBEAT;
        heatbeat_buffer[MSG_LENGTH] = 17;
        memcpy(&heatbeat_buffer[SOURCE_ID_IDX],&g_com_map[DEV_ID],2);
        memcpy(&heatbeat_buffer[DEST_ID_IDX],&wangguanid,2);
        memcpy(&heatbeat_buffer[ANC_HB_VERSION_IDX],&g_com_map[VERSION],2);
        memcpy(&heatbeat_buffer[ANC_HB_UWBPOLLTIME_IDX],&g_com_map[UWBPOLLTIME_MS_IDX],2);
        heatbeat_buffer[ANC_HB_UWBPOWER_IDX] = g_com_map[POWER];
        heatbeat_buffer[ANC_HB_LORAPOWER_IDX] = g_com_map[LORA_POWER];
        heatbeat_buffer[ANC_HB_BATTARY_IDX] = bat_percent;
        checksum1=Checksum_u16(heatbeat_buffer,17);
        memcpy(&heatbeat_buffer[17],&checksum1,2);
        Radio.Send(heatbeat_buffer, 19);
        Delay_Ms(60);
    }
}
static uint8_t lora_sendbuffer[200];
uint8_t flag_writepara_needreset;
static uint8_t data_length;
void LoraSendComMap(uint8_t cmd)
{
	uint16_t checksum = 0;
    data_length = 80;
	lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_READPARARESP;
	lora_sendbuffer[MSG_LENGTH] = data_length+11;
    memcpy(&lora_sendbuffer[SOURCE_ID_IDX],&g_com_map[DEV_ID],2);
    memcpy(&lora_sendbuffer[DEST_ID_IDX],&wg_report_id,2);
	lora_sendbuffer[RP_CMD_IDX] = cmd;
	lora_sendbuffer[RP_INDEX_IDX] = 2;
	lora_sendbuffer[RP_LEN_IDX] = data_length;
	memcpy(&lora_sendbuffer[9], &g_com_map[1], data_length);
	checksum = Checksum_u16(lora_sendbuffer,11+data_length);
	memcpy(&lora_sendbuffer[11+data_length],&checksum,2);
    Radio.Send(lora_sendbuffer,data_length+13);
    Delay_Ms(160);
}
void LoraRspWriteCommap(uint8_t index)
{
	uint16_t checksum = 0;
    data_length = 2;
	lora_sendbuffer[MSG_TYPE_IDX] = LORA_MSGTYPE_READPARARESP;
	lora_sendbuffer[MSG_LENGTH] = data_length+9;
    memcpy(&lora_sendbuffer[SOURCE_ID_IDX],&g_com_map[DEV_ID],2);
    memcpy(&lora_sendbuffer[DEST_ID_IDX],&wg_report_id,2);
	lora_sendbuffer[RP_CMD_IDX] = WGRSP_RWTAG_WRITE;
	lora_sendbuffer[RP_INDEX_IDX] = index;
	lora_sendbuffer[RP_LEN_IDX] = data_length;
	memcpy(&lora_sendbuffer[7], &g_com_map[index/2], data_length);
	checksum = Checksum_u16(lora_sendbuffer,9+data_length);
	memcpy(&lora_sendbuffer[9+data_length],&checksum,2);
    Radio.Send(lora_sendbuffer,data_length+11);
    Delay_Ms(160);
}
static uint8_t rec_index;
uint16_t rec_value;
void LoraSyncRecPoll(void) 
{ 
    if(++sync_interval_count>=sync_interval_s)
    {
        sync_interval_count = 0;
        flag_sync_success = 0;
        
        SwitchLoraSettings(SYNC_CHANNEL_FRQ,7,22); //Çл»lora½ÓÊÕƵµã//ÒòΪÊÇ´Ólorasleep»½Ðѵģ¬¿ÉÄÜÕâÀï»áÓõÄʱ¼ä¶à£¬ÐèÒª¿¼ÂÇ
       
        start_count=HAL_LPTIM_ReadCounter(&hlptim1);     //µÈ´ý½ÓÊÕÍê³ÉÖ±µ½³¬Ê±
        poll_timeout=sync_listen_ms*10; //µ¥Î»0.1ms
        end_count=start_count+(poll_timeout<<2);
        if(end_count>=32768)
        {end_count-=32768;}
        current_count=HAL_LPTIM_ReadCounter(&hlptim1);
        SetFlagLoraWaitSync(1);
       
        Radio.Rx(sync_listen_ms);
        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_SET);
       while(GetFlagLoraWaitSync())
       {

       }
       HAL_GPIO_WritePin(GPIOB, GPIO_PIN_12, GPIO_PIN_RESET);
       if(flag_sync_success)  //ͬ²½³É¹¦
       {   uint16_t wangguanid,checksum1;
         //  printf("ͬ²½³É¹¦\r\n");
           lora_recbuffer_len = GetLoraBufferAndLen(lora_recbuffer,&rec_sync_count);//»ñÈ¡LORAÊÕµ½µÄÊý¾ÝºÍ³¤¶È
           checksum1=Checksum_u16(lora_recbuffer,lora_recbuffer_len-2);
        if(!memcmp(&checksum1,&lora_recbuffer[lora_recbuffer_len-2],2))  //CRC16УÑéÊÇ·ñͨ¹ý
        {
           memcpy(&wangguanid,&lora_recbuffer[SOURCE_ID_IDX],2);
           HeartbeatPoll(wangguanid);
           LED_LG_ON;
           sync_lost_count = 0;
           if(lora_recbuffer[MSG_TYPE_IDX]==LORA_MSGTYPE_SYNC)
           { uint16_t wg_sendtime_100us,pwanc_value;
               uint8_t pwanc_index;
               memcpy(&g_com_map[SYNCWG_ID_IDX],&lora_recbuffer[SOURCE_ID_IDX],2);
               memcpy(&g_com_map2[SYNCWG_ID_IDX],&lora_recbuffer[SOURCE_ID_IDX],2);
               memcpy(&wg_report_id,&lora_recbuffer[SOURCE_ID_IDX],2);
               memcpy(&wg_sendtime_100us,&lora_recbuffer[SYSTIME_IDX],2);
               synclock_zero_count = rec_sync_count-(wg_sendtime_100us+158)*3.27;//¼õÈ¥17ms ÒòΪlora·¢ËÍ»áÑÓ³Ù17msÊÕµ½
               while(synclock_zero_count<0)
                   synclock_zero_count+=32768;
               sync_lost_count = 0;
                if(lora_recbuffer[RANGE_STATE_IDX])
                {
                    sync_state = Sync_Range;
                }else{
                    sync_state = Sync_Sleep;
                }
                SyncStateSwitch();
                if(!memcmp(&dev_id,&lora_recbuffer[DEST_ID_IDX],2))
                switch(lora_recbuffer[SUBMSGTYPE_IDX]) //×Ó״̬º¯Êý
                {
                    case SUBMSG_READ_ANCPARA:
                        heartbeat_count=HEATBEAT_UPDATE_TIME_S-2;
                        LoraSendComMap(SUBMSG_READ_ANCPARA);
                        config_state_count = CONFIG_REMAIN_TIME;
                        break;
                    case SUBMSG_WRITE_ANCPARA:
                        rec_index = lora_recbuffer[WRITEPARA_INDEX_IDX];
                        memcpy(&rec_value,&lora_recbuffer[WRITEPARA_VALUE_IDX],2);
                        g_com_map[rec_index/2] = rec_value;
                        LoraRspWriteCommap(rec_index);
                        save_com_map_to_flash();
                        flag_writepara_needreset = 1;
                        break;
                    case SUBMSG_UPDATE_ANC:                
                        LoraUp_Poll();
                        break;
                }
           }
       }else{
           if(sync_lost_count++>SYNCLOST_THRES)
            {
                sync_state = Sync_Lost;
                SyncStateSwitch();
            }  
       }
       }else{
         //  LED_LR_ON;
        //   printf("ͬ²½Ê§°Ü\r\n");
            if(sync_lost_count++>SYNCLOST_THRES)
            {
                sync_state = Sync_Lost;
                SyncStateSwitch();
            }           
       }
       Radio.Sleep();
     //  LED_LR_OFF;
       LED_LG_OFF;
   }
  //  Radio.Sleep( ); //Èç¹û³¬Ê±¾Ísleep
}
////////////////////////////Ãâ²¼Ïß²â¾àÏà¹Ø´úÂë


#define TAG_KEEPTIMES    30 //±êÇ©´æ»îʱ¼ä
#define MAX_TAG_LIST_NUM 200 //ͬʱͨѶ±êÇ©×î´óÊýÁ¿

static uint8_t uwb_sendbuffer[150];
static uint8_t rx_buffer[150];

uint16_t  taglist_total_num,taglist_current_index;  //µ±Ç°Áбí×ÜÊýÁ¿
static uint8_t  taglist_keeptime[MAX_TAG_LIST_NUM]; //ÿ¸ö±êÇ©´æ»îʱ¼ä
static uint16_t taglist_id[MAX_TAG_LIST_NUM],taglist_dist[MAX_TAG_LIST_NUM]; //±êÇ©¾àÀëºÍ±êÇ©µÄID


static u8 frame_seq_nb,frame_len;

static uint32_t status_reg = 0;
extern int32_t dwt_readcarrierintegrator(void);
static int32_t test2;
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
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;
}
//×¢Ò⣺ÒòΪÐÂ×¢²áµÄ±êÇ©²¢²»ÖªµÀ»ùÕ¾ÒѾ­ÊÕµ½ÁË£¬ËùÒÔÒªÔÚÏÂÒ»°üÁ¢¿Ì½«ÐµıêÇ©ID·¢³ö£¬·ñÔò±êÇ©»áÒ»Ö±ÇÀÕ¼»ùÕ¾µÄpoll°ü¡£
//·½·¨Êǽ«ÐÂ×¢²áµÄ±êÇ©Ìí¼Óµ½Ï´η¢Ë͵ıêÇ©ÁбíÍ·²¿£¬ÆäËû±êÇ©ÒÀ´ÎÏòºóŲ¶¯¡£
uint16_t AddNewTagIntoList(uint16_t tagid)  
{                                       
     u16 newindex_cal = taglist_current_index+25;//µ±Ç°Ë÷Òý+25£¬ÎªÏ´ÎË÷ÒýÒª·¢³öµÄÖµµÄ×î´óÖµ£¬Èç¹û×ÜÊýСÓÚÕâ¸öÖµ£¬¼´¿É·Åµ½×îºóÃæ¡£
      if(newindex_cal>=taglist_total_num) //Èç¹ûµ±Ç°Ë÷ÒýÊÇ×îºóÒ»°ü£¬ÄÇô¾Í¼Óµ½×ÜË÷ÒýµÄºóÃ棬Èç¹û²»ÊǾͼӵ½ÏÂÒ»´ÎÆðʼË÷ÒýλÖá£
      {
        newindex_cal = taglist_total_num;
          
      }else{           
         for(u16 i=taglist_total_num;i>newindex_cal;i--)  //´ÓºóÍùǰŲµØ·½
            {
                taglist_id[i]=taglist_id[i-1];
                taglist_dist[i]=taglist_dist[i-1];
                taglist_keeptime[i]=taglist_keeptime[i-1];
            }          
      }
      taglist_total_num++;
      taglist_id[newindex_cal] = tagid;
      taglist_dist[newindex_cal] = 0;
      taglist_keeptime[newindex_cal] = TAG_KEEPTIMES;
      return newindex_cal;
}
uint16_t CmpTagInList(uint16_t tagid)
{
    u16 temp;
	for(u8 i=0;i<taglist_total_num;i++)
	{
		if(memcmp(&tagid,&taglist_id[i],2)==0)
        {
            taglist_keeptime[i] = TAG_KEEPTIMES;
			return i;
        }
	}
    if(taglist_total_num<MAX_TAG_LIST_NUM)
    temp = AddNewTagIntoList(tagid);
	return temp;
}
static uint32_t anc_pollrx[MAX_NEARBASE_NUM],anc_resptx[MAX_NEARBASE_NUM],tag_resprx[MAX_NEARBASE_NUM];
static int32_t anc_clockoffset[MAX_NEARBASE_NUM];
static int16_t anc_distoffset[MAX_NEARBASE_NUM];
static 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;
}
static float clockOffsetRatio;
#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)

#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)
static float clockOffsetRatio;
static double rtd_init, rtd_resp;
static double tof,distance;
#define SPEED_OF_LIGHT 299702547
u16 count1,count2;
static void CalculateDists(void)   //¼ÆËã¾àÀë
{
    for(int i=0;i<MAX_NEARBASE_NUM+10;i++) //ÒòΪÔö¼ÓÁË5¸öÇÀռλÖã¬ËùÒÔ×îÖյļÆËãÊÇ×ÜÊýÁ¿+10.¿¼ÂÇʵ¼ÊÇÀÕ¼¿ÉÄܳ¬¹ý5¸ö
    {
        //rec_anc_signalpower[i] = exsistbase_list[i];
      if(taglist_keeptime[i+taglist_current_index]==TAG_KEEPTIMES)
      {

         // 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)
          {
              count2++;
            taglist_dist[i+taglist_current_index] = distance*100+anc_distoffset[i];
          }
      }else{
         // nearbase_distlist[i] = 0x1ffff;
      }
    
    }
}
void testprogram(void)
{

}
void TagListUpdate(void)
{
	uint16_t i,j=0,temp[TAG_NUM_IN_SYS];
	for(i=0;i<taglist_total_num;i++)
	{
		if(taglist_keeptime[i]-->0)
		{
			taglist_id[j]=taglist_id[i];
			taglist_keeptime[j++]=taglist_keeptime[i];
		}
	}
	taglist_total_num=j;
}
extern uint16_t verfint_cal;
u16 rec_nearbaseid,temp_index,adc_val1,adc_val_min=0xffff;
static u8 rec_tag_index,tag_num_tosend;
extern uint8_t Anchor_Vcc_Flag;
extern float bat_volt;
uint16_t AD_value[500],ti;
extern float Height;
u8 rec_tag_num,temp_tag_num,kp1;
void MBXANCPoll(void)  //½«Òª·¢µÄ±êÇ©ÐòºÅ£¬ÔÚÁбíÖеÄλÖÃ
{
    temp_tag_num = 0;
    tag_num_tosend = taglist_total_num-taglist_current_index;
    if(tag_num_tosend>25)
    {
        tag_num_tosend = 25;
    }
    count1++;
    ti = 0;
    dwt_setrxaftertxdelay(300);//ÉèÖ÷¢ËÍÍê³Éºó¿ªÆô½ÓÊÕÑÓ³Ùʱ¼ä
    dwt_setrxtimeout(0);//½ÓÊÕ³¬Ê±Ê±¼ä
    memcpy(&uwb_sendbuffer[TAG_ID_IDX],&dev_id,2);   
    uwb_sendbuffer[GROUP_ID_IDX] = group_id;
	uwb_sendbuffer[BATTARY_IDX] = bat_percent;
    uwb_sendbuffer[SEQUENCE_IDX] = frame_seq_nb++;
	uwb_sendbuffer[NEARBASENUM_INDEX] = tag_num_tosend;
    memcpy(&uwb_sendbuffer[NEARBASEID_INDEX],&taglist_id[taglist_current_index],tag_num_tosend*2);
    memcpy(&uwb_sendbuffer[NEARBASEID_INDEX+tag_num_tosend*2],&taglist_dist[taglist_current_index],tag_num_tosend*2);
	uwb_sendbuffer[MESSAGE_TYPE_IDX] = MBX_POLL;
    dwt_writetxdata(13+4*tag_num_tosend, uwb_sendbuffer, 0);//½«Poll°üÊý¾Ý´«¸øDW1000£¬½«ÔÚ¿ªÆô·¢ËÍʱ´«³öÈ¥
	dwt_writetxfctrl(13+4*tag_num_tosend, 0);//ÉèÖó¬¿í´ø·¢ËÍÊý¾Ý³¤¶È
    
	dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//¿ªÆô·¢ËÍ£¬·¢ËÍÍê³ÉºóµÈ´ýÒ»¶Îʱ¼ä¿ªÆô½ÓÊÕ£¬µÈ´ýʱ¼äÔÚdwt_setrxaftertxdelayÖÐÉèÖÃ	
    if(Anchor_Vcc_Flag)
    {
        READC_Init();
        adc_val_min=0xffff;
        verfint_cal = *(__IO uint16_t *)(0X1FF80078);  
    }        
    start_count=HAL_LPTIM_ReadCounter(&hlptim1);
    poll_timeout=tag_num_tosend*SLOT_SCALE+10; //µ¥Î»0.1ms
	end_count=start_count+(poll_timeout<<2);
	if(end_count>=32768)
	{end_count-=32768;}
    current_count=HAL_LPTIM_ReadCounter(&hlptim1);
    while(current_count<end_count||current_count>end_count+15000)
        {
				current_count=HAL_LPTIM_ReadCounter(&hlptim1);
					while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//²»¶Ï²éѯоƬ״ֱ̬µ½³É¹¦½ÓÊÕ»òÕß·¢Éú´íÎó
					{ 
                        if(Anchor_Vcc_Flag)
                        {                           
                            adc_val1 = Get_ADC_Value();
                            AD_value[ti++] = adc_val1;
                            if(ti>=500)
                                ti = 400;
                            if(adc_val1<adc_val_min)
                            {
                                adc_val_min= adc_val1;
                            }
                        }
							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();
                            poll_tx_ts = get_tx_timestamp_u64();										//»ñµÃPOLL·¢ËÍʱ¼äT1
                            resp_rx_ts = get_rx_timestamp_u64();
                            dwt_setrxtimeout(0);//DELAY_BETWEEN_TWO_FRAME_UUS*(nearbase_num+1-recbase_num)+10);//É趨½ÓÊÕ³¬Ê±Ê±¼ä£¬0λûÓг¬Ê±Ê±¼ä
							dwt_rxenable(0);//´ò¿ª½ÓÊÕ
							if (rx_buffer[MESSAGE_TYPE_IDX] == MBX_RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,2)&&rx_buffer[GROUP_ID_IDX]==group_id) //ÅжϽÓÊÕµ½µÄÊý¾ÝÊÇ·ñÊÇresponseÊý¾Ý
							{	
								memcpy(&rec_nearbaseid,&rx_buffer[ANCHOR_ID_IDX],2);
                                rec_tag_index = CmpTagInList(rec_nearbaseid);
                                temp_index =rec_tag_index -taglist_current_index;
                                SetANCTimestap(temp_index,&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]);
                                temp_tag_num++;
                            }

                        }else{
						dwt_write32bitreg(SYS_STATUS_ID,SYS_STATUS_RXFCG| SYS_STATUS_ALL_RX_ERR);
                        dwt_rxenable(0);	
															
						}	                      
                    } 
    LED_TB_OFF;        
    dwt_forcetrxoff();
    rec_tag_num = temp_tag_num;
    CalculateDists();            
    TagListUpdate();
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
    taglist_current_index+=tag_num_tosend;
    if(taglist_current_index>=taglist_total_num||taglist_total_num<=25) //Èç¹û±êÇ©µ±Ç°index´óÓÚ×Üindex£¬»òÕß×Ü»ùÕ¾ÊýÁ¿Ð¡ÓÚµÈÓÚ25£¬Ôò´Ó0¿ªÊ¼index¡£
        taglist_current_index = 0;
    if(Anchor_Vcc_Flag)
    {
        HAL_GPIO_WritePin(GPIOA, GPIO_PIN_12, GPIO_PIN_RESET);
        Anchor_Vcc_Flag=0;
        MX_ADC_DeInit();
        bat_volt = 3*(float)verfint_cal/AD_value[21]-0.1;
        bat_percent=bat_volt*100-200;
    }
    for(uint8_t i=0;i<taglist_total_num;i++)
    {
        if(taglist_id[i] == 0x33)
        {
            kp1 = taglist_keeptime[i];
        }
    }
#ifdef _USE_BAR_
 //if(taglist_total_num>0)
 {
    GetPressAndHeight();  
    intheight = Height*100;   
 }
#endif        
}