代码排序后

yincheng.zhong

2023-08-16 56900ed5464e7cf4215052c706e8c22b00147e4a

 Ô´Âë/ºËÐÄ°å/Src/application/dw_app.c

@@ -1,156 +1,52 @@

/*! ----------------------------------------------------------------------------
 *  @file    main.c
 *  @brief   Double-sided two-way ranging (DS TWR) initiator example code
 *
 *         
 *
 * @attention
 *
 * Copyright 2015 (c) Decawave Ltd, Dublin, Ireland.
 *
 * All rights reserved.
 *
 * @author Decawave
 */

#include <string.h>
#include "dw_app.h"
#include "deca_device_api.h"
#include "deca_regs.h"
#include "dw_driver.h"
#include "Spi.h"
#include "led.h"
#include "serial_at_cmd_app.h"
#include "Usart.h"
#include "global_param.h"
#include "filters.h"
#include <stdio.h>
#include "beep.h"


/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100

/* Default antenna delay values for 64 MHz PRF. See NOTE 1 below. */
#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

/* Delay between frames, in UWB microseconds. See NOTE 4 below. */
/* This is the delay from the end of the frame transmission to the enable of the receiver, as programmed for the DW1000's wait for response feature. */
#define POLL_TX_TO_RESP_RX_DLY_UUS 150
/* This is the delay from Frame RX timestamp to TX reply timestamp used for calculating/setting the DW1000's delayed TX function. This includes the
 * frame length of approximately 2.66 ms with above configuration. */
#define RESP_RX_TO_FINAL_TX_DLY_UUS 400
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 600

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

#define 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

#define GROUP_ID_IDX               0
#define ANCHOR_ID_IDX             1
#define TAG_ID_IDX                5
#define MESSAGE_TYPE_IDX          9	
#define DIST_IDX                      10
#define ANC_TYPE_IDX                14
#define BATTARY_IDX                  15
#define BUTTON_IDX                  16

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

/*------------------------------------ Variables ------------------------------------------*/
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
#include "ADC.h"
enum enumtagstate
{
    DISCPOLL,
    GETNEARMSG,
    NEARPOLL,
} tag_state=GETNEARMSG;
static dwt_config_t config = {
   2,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
   DWT_PLEN_128,    /* Preamble length. */
   DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
   9,               /* TX preamble code. Used in TX only. */
   9,               /* RX preamble code. Used in RX only. */
   0,               /* Use non-standard SFD (Boolean) */
   DWT_BR_6M8,      /* Data rate. */
   DWT_PHRMODE_STD, /* PHY header mode. */
   (129 + 8 - 8)    /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
    2,               /* Channel number. */
    DWT_PRF_64M,     /* Pulse repetition frequency. */
    DWT_PLEN_128,    /* Preamble length. */
    DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
    9,               /* TX preamble code. Used in TX only. */
    9,               /* RX preamble code. Used in RX only. */
    1,               /* Use non-standard SFD (Boolean) */
    DWT_BR_6M8,      /* Data rate. */
    DWT_PHRMODE_STD, /* PHY header mode. */
    (129 + 8 - 8)    /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
};
static uint8_t tx_poll_msg[20] = {0};
static uint8_t tx_sync_msg[14] = {0};
static uint8_t tx_final_msg[60] = {0};
static uint8_t tx_resp_msg[22] = {0};
static uint8_t tx_nearpoll_msg[80] = {0};
static uint8_t tx_nearresp_msg[90] = {0};
static uint8_t tx_nearfinal_msg[80] = {0};

/* Frames used in the ranging process. See NOTE 2 below. */
static uint8_t tx_poll_msg[19] = {0};
//static uint8_t rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8_t tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_resp_msg[16] = {0};
//static uint8_t rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
	
/* Frame sequence number, incremented after each transmission. */
static uint32_t frame_seq_nb = 0;	
	
/* Hold copy of status register state here for reference, so reader can examine it at a breakpoint. */
static uint8_t tx_near_msg[80] = {0};

uint32_t frame_seq_nb = 0, frame_seq_nb2 = 0;
static uint32_t status_reg = 0;
	
/* Buffer to store received response message.
 * Its size is adjusted to longest frame that this example code is supposed to handle. */
#define RX_BUF_LEN       24
static uint8_t rx_buffer[RX_BUF_LEN];
	
/* Time-stamps of frames transmission/reception, expressed in device time units.
 * As they are 40-bit wide, we need to define a 64-bit int type to handle them. */
static uint64_t poll_tx_ts;
static uint64_t resp_rx_ts;
static uint64_t final_tx_ts;
	
/* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */
static uint8_t rx_buffer[100];
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;

static double tof;
	
uint32_t anchor_dist_last_frm[TAG_NUM_IN_SYS];	
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; 
uint32_t g_UWB_com_interval = 0;
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器

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


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

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

/*! ------------------------------------------------------------------------------------------------------------------
 * @fn final_msg_set_ts()
 *
 * @brief Fill a given timestamp field in the final message with the given value. In the timestamp fields of the final
 *        message, the least significant byte is at the lower address.
 *
 * @param  ts_field  pointer on the first byte of the timestamp field to fill
 *         ts  timestamp value
 *
 * @return none
 */
static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
{
    int i;
@@ -219,58 +94,61 @@
        *ts += ts_field[i] << (i * 8);
    }
}
extern int32_t tagdist_list[TAG_NUM_IN_SYS];
void TagDistClear(void)
{
   static uint16_t clear_judge_cnt;
   uint16_t i;
   if(clear_judge_cnt++>1000)  //设定1S分频，每秒进一次。判断标志位大于等于2，2s没收到数据就把数据变成0xffff，不触发警报。
   {
      clear_judge_cnt=0;
      for(i=0;i<255;i++)
      {
         g_flag_Taggetdist[i]++;
         if(g_flag_Taggetdist[i]>=2)
         {
            g_Tagdist[i]=0xffff;
         }
      }
   }
    static uint16_t clear_judge_cnt;
    uint16_t i;
    for(i=0; i<255; i++)
    {
        g_flag_Taggetdist[i]++;
        if(g_flag_Taggetdist[i]>=2)
        {
            tagdist_list[i]=0x1ffff;
        }
    }
}
void Dw1000_Init(void)
{
   /* Reset and initialise DW1000.
    /* Reset and initialise DW1000.
     * For initialisation, DW1000 clocks must be temporarily set to crystal speed. After initialisation SPI rate can be increased for optimum
     * performance. */
    Reset_DW1000();//重启DW1000 /* Target specific drive of RSTn line into DW1000 low for a period. */
    status_reg = dwt_read32bitreg(SYS_STATUS_ID);
    dwt_initialise(DWT_LOADUCODE);//初始化DW1000
   Spi_ChangePrescaler(SPIx_PRESCALER_FAST);   //设置为快速模式
    status_reg = dwt_read32bitreg(SYS_STATUS_ID);
    Spi_ChangePrescaler(SPIx_PRESCALER_FAST);   //设置为快速模式

    /* Configure DW1000. See NOTE 6 below. */
    dwt_configure(&config);//配置DW1000
	

	
    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);

    /* Apply default antenna delay value. See NOTE 1 below. */
    dwt_setrxantennadelay(RX_ANT_DLY);      //设置接收天线延迟
    dwt_settxantennadelay(TX_ANT_DLY);      //设置发射天线延迟

    /* Set expected response's delay and timeout. See NOTE 4 and 5 below.
     * As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
    dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);         //设置发送后开启接收，并设定延迟时间
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);                  //设置接收超时时间
    //设置接收超时时间
}
void Dw1000_App_Init(void)
{
//g_com_map[DEV_ID] = 0x0b;
   tx_poll_msg[MESSAGE_TYPE_IDX]=POLL;
   tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE;
   tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
   memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 4);
   memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 4);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 4);
	
}	
uint16_t Checksum_u16(uint8_t* pdata, uint32_t len) 
    tx_poll_msg[MESSAGE_TYPE_IDX]=POLL;
    tx_resp_msg[MESSAGE_TYPE_IDX]=RESPONSE;
    tx_final_msg[MESSAGE_TYPE_IDX]=FINAL;
    tx_sync_msg[MESSAGE_TYPE_IDX]=SYNC;

    memcpy(&tx_poll_msg[TAG_ID_IDX], &dev_id, 2);
    memcpy(&tx_final_msg[TAG_ID_IDX], &dev_id, 2);
    memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &dev_id, 2);
    memcpy(&tx_sync_msg[ANCHOR_ID_IDX], &dev_id, 2);
    memcpy(&tx_nearresp_msg[ANCHOR_ID_IDX], &dev_id, 2);
    memcpy(&tx_nearpoll_msg[TAG_ID_IDX], &dev_id, 2);
    memcpy(&tx_nearfinal_msg[TAG_ID_IDX], &dev_id, 2);
}
uint16_t Checksum_u16(uint8_t* pdata, uint32_t len)
{
    uint16_t sum = 0;
    uint32_t i;
@@ -280,314 +158,844 @@
    return sum;
}

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

extern uint8_t g_start_send_flag;
u8 g_start_sync_flag;
void SyncPoll(u8 sync_seq)
{   u8 result;
//   g_start_sync_flag=1;   //中断模式，退出终端后，需要重新来过
    dwt_forcetrxoff();      //关闭接收，以防在RX ON çŠ¶æ€


    tx_sync_msg[SYNC_SEQ_IDX]=sync_seq;
    memcpy(&tx_sync_msg[ANCHOR_ID_IDX],&dev_id,4);
    dwt_writetxdata(sizeof(tx_sync_msg), tx_sync_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
    dwt_writetxfctrl(sizeof(tx_sync_msg), 0);//设置超宽带发送数据长度
    dwt_starttx(DWT_START_TX_IMMEDIATE);
    if(result==0)
    {
        while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
        { };
    }
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清楚标志位

    dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
    dwt_rxenable(0);//打开接收
}
uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
u32 hex_dist;
int32_t hex_dist;
u16 checksum;
void Tag_App(void)//发送模式(TAG标签)
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=0,last_nearbase_num,next_nearbase_num;
u16 nearbaseid_list[MAX_NEARBASE_NUM],mainbase_id,true_nearbase_idlist[MAX_NEARBASE_NUM];
int32_t mainbase_dist,nearbase_distlist[MAX_NEARBASE_NUM],true_nearbase_distlist[MAX_NEARBASE_NUM],true_exsistbase_list[MAX_NEARBASE_NUM];
uint8_t trygetnearmsg_times;
u16 current_slotnum,total_slotnum,target_time,last_time;
u32 rec_tagpos_binary;
extern uint32_t tagpos_binary;
void SetNextPollTime(u16 time)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   u32 start_poll;
   u8 i;
   //LED0_ON;
   g_Resttimer=0;
   GPIO_ResetBits(SPIx_GPIO, SPIx_CS);
   delay_us(2500);
   GPIO_SetBits(SPIx_GPIO, SPIx_CS);
   tag_succ_times = 0;
   tx_poll_msg[BATTARY_IDX] = Get_Battary();
   tx_poll_msg[BUTTON_IDX] = !READ_KEY0;
   dwt_forcetrxoff();
   for(i=0;i<REPOET_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);//设置超宽带发送数据长度
    current_slotnum++;
    if(current_slotnum==total_slotnum)
        current_slotnum-=total_slotnum;
    //time=5;
    target_time=((current_slotnum*g_com_map[COM_INTERVAL])+time);

   /* 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)))//不断查询芯片状态直到成功接收或者发生错误
   { if(time32_incr - start_poll>20)
      NVIC_SystemReset();
      UART_CheckReceive();
      UART_CheckSend();
   };
    last_time=target_time;
    if(target_time>=990&&target_time<992)
    {
        current_slotnum++;
        target_time+=g_com_map[COM_INTERVAL];
        tagslotpos=GetRandomSlotPos(rec_tagpos_binary|tagpos_binary);
        tyncpoll_time = (tagslotpos--%max_slotpos)*slottime;
    }
    if(target_time>=1000)
    {
        target_time-=1000;
    }
    if(target_time<0)
    {
        target_time+=1000;
    }

   /* 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);//清楚寄存器标志位

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

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


      /* Check that the frame is the expected response from the companion "DS TWR responder" example.
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
		
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&!memcmp(&rx_buffer[TAG_ID_IDX],&dev_id,4)) //判断接收到的数据是否是response数据
      {
         /* 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
			
         memcpy(&anchor_dist_last_frm[tag_id], &rx_buffer[DIST_IDX], 4);
         memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 4);
         /* Compute final message transmission time. See NOTE 9 below. */
         final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算final包发送时间，T5=T4+Treply2
         dwt_setdelayedtrxtime(final_tx_time);//设置final包发送时间T5

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

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

         /* Write and send final message. See NOTE 7 below. */
		
         dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//将发送数据写入DW1000
         dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
			
         tag_succ_times++;
			
         /* 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). */

         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();
   if(tag_succ_times<REPOET_ANC_NUM_MIN)
   {
   random_delay_tim =time32_incr&0x8f+7;
   deca_sleep(random_delay_tim);
   }
   //LED0_BLINK;
   RTC_SET_ALARM(1);
   /* Execute a delay between ranging exchanges. */
	
u8 FindNearBasePos(u16 baseid)
{
    u8 i;
    for(i=0; i<last_nearbase_num; i++)
    {
        if(baseid==nearbaseid_list[i])
            return i;
    }
    return i;
}
extern uint8_t g_start_send_flag;
u8 recbase_num=0;
#define CHANGE_BASE_THRESHOLD  5
//void NearAncSelect(void)
//{static u16 last_mainbase_id,change_base_count;
//   int32_t nearbase_mindist=99999, nearbase_minpos;
//   u8 i;
//   for(i=0;i<recbase_num-1;i++)
//   {
//      if(nearbase_mindist>nearbase_distlist[i])
//      {
//         nearbase_mindist=nearbase_distlist[i];
//         nearbase_minpos=i;
//      }
//   }
//   if(nearbase_mindist<mainbase_dist-THRESHOLD_CHANGE_MAINBASE_DIST)
//   {
//      if(last_mainbase_id==nearbaseid_list[nearbase_minpos])
//      {
//         change_base_count++;
//         if(change_base_count>CHANGE_BASE_THRESHOLD)
//         {
//            mainbase_id=last_mainbase_id;
//            tag_state=GETNEARMSG;
//         }
//      }else{
//         change_base_count=0;
//      }
//      last_mainbase_id=nearbaseid_list[nearbase_minpos];
//   }else{
//      change_base_count=0;
//   }
//}
uint8_t GetRandomValue(void)
{
    uint8_t random_value=0,temp_adc,i;
    for(i=0; i<8; i++)
    {
        temp_adc=Get_ADC_Value();
        random_value=random_value|((temp_adc&0x01)<<i);
    }
    return random_value;
}
u8 GetRandomSlotPos(uint32_t emptyslot)
{
    u8 i,temp_value;
    temp_value = GetRandomValue();
    for(i=temp_value%max_slotpos; i<max_slotpos; i++)
    {
        if(((emptyslot>>i)&0x1)==0)
        {
            return i;
        }
    }

    for(i=1; i<max_slotpos; i++)
    {
        if(((emptyslot>>i)&0x1)==0)
        {
            return i;
        }
    }
    return max_slotpos-1;
}
dwt_rxdiag_t d1;
u8 test=0;
u8 anclost_times=0;
u8 exsistbase_list[MAX_NEARBASE_NUM],report_num,get_newbase=0;
u16 temp_sync_timer1,temp_sync_timer2;
double firstpath_power, rx_power;
double f1, f2, r1, r2;
uint16_t F1,F2,F3,N,C;
double B = 131072;
double A = 121.74;
double min_power;
double LOS(dwt_rxdiag_t *dia) {
    F1 = dia->firstPathAmp1;
    F2 = dia->firstPathAmp2;
    F3 = dia->firstPathAmp3;
    N = dia->rxPreamCount;
    C = dia->maxGrowthCIR;

    firstpath_power=10* log10((F1*F1+F2*F2+F3*F3)/(N*N))-A;
    rx_power=10*log10(C*B/(N*N))-A;

    // min_power =  - 10 * log10((F1 *F1 + F2 * F2 + F3 * F3) / (C *B));
    return min_power;
}

u8 nearmsg_mainbase=0,rec_tagpos_emptylist[32];

int8_t correction_time,new_tagid=0;
extern uint8_t sync_seq;
u16 taglist_num=0,taglist_pos;
u16 tagid_list[TAG_NUM_IN_SYS];
u8 tagofflinetime[TAG_NUM_IN_SYS];
int32_t tagdist_list[TAG_NUM_IN_SYS];
void TagListUpdate(void)
{
    u16 i,j=0,temp[TAG_NUM_IN_SYS];
    for(i=0; i<taglist_num; i++)
    {
        if(tagofflinetime[i]++<QUIT_SLOT_TIME)
        {
            tagid_list[j]=tagid_list[i];
            tagofflinetime[j++]=tagofflinetime[i];
        }
    }
    taglist_num=j;
}
u16 CmpTagInList(u16 tagid)
{   u16 i;
    for(i=0; i<taglist_num; i++)
    {
        if(memcmp(&tagid,&tagid_list[i],2)==0)
            break;
    }
    return i;
}
u8 misdist_num[TAG_NUM_IN_SYS],seize_anchor,waittagconfig_reponse,motorstate=0;
uint32_t frame_len;
uint32_t resp_tx_time;
uint8_t rec_nearbase_num,anc_report_num;
void Anchor_RecPoll(void)
{
    tmp_time=TIM3->CNT;
    memcpy(&tx_resp_msg[ANCTIMEMS],&sync_timer,2);
    memcpy(&tx_resp_msg[ANCTIMEUS],&tmp_time,2);
    memcpy(&tx_resp_msg[TAGSLOTPOS],&taglist_pos,1);

    poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2

    resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
    dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
    dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);//设置发送完成后开启接收延迟时间
    dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间

    memcpy(&tx_resp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
    memcpy(&tx_resp_msg[TAG_ID_IDX],&tag_id_recv,2);

    dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
    dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
    result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收


    frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
    if(result==0)
    {
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///不断查询芯片状态直到接收成功或者出现错误
        { };
    }
    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] == DISCOVERFINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //判断是否为Final包
        {
            uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
            uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
            double Ra, Rb, Da, Db;
            int64_t tof_dtu;
            resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3
            final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6
            final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1，T4，T5
            final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
            final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
            poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
            resp_tx_ts_32 = (uint32_t)resp_tx_ts;
            final_rx_ts_32 = (uint32_t)final_rx_ts;
            Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
            Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
            Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
            Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);//Treply1 = T3 - T2
            tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计算公式
            tof = tof_dtu * DWT_TIME_UNITS;
            distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
            dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
            dist_cm = dist_no_bias * 100; //dis ä¸ºå•ä½ä¸ºcm的距离
            /*--------------------------以下为非测距逻辑------------------------*/
            //dist_cm=33000;

            LED0_BLINK; //每成功一次通讯则闪烁一次
            dis_after_filter=dist_cm;
            hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
            if(hex_dist>-1000&&hex_dist<200000)
            {
                g_flag_Taggetdist[taglist_pos]=0;
                if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)
                {
                    misdist_num[taglist_pos]=0;
                    tagdist_list[taglist_pos] = hex_dist;
                    his_dist[taglist_pos]=hex_dist;
                    g_Tagdist[taglist_pos]=hex_dist;
#ifndef USART_INTEGRATE_OUTPUT
                    usart_send[2] = 1;//正常模式
                    usart_send[3] = 17;//数据段长度
                    usart_send[4] = frame_seq_nb2;//数据段长度
                    memcpy(&usart_send[5],&tag_id_recv,2);
                    memcpy(&usart_send[7],&dev_id,2);
                    memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
                    usart_send[13] = battary;
                    usart_send[14] = button;
                    usart_send[15] = firstpath_power;
                    usart_send[16] = (rx_power-firstpath_power)*10;
                    usart_send[17] = tag_frequency;
                    usart_send[18] = tag_slotpos;
                    checksum = Checksum_u16(&usart_send[2],17);
                    memcpy(&usart_send[19],&checksum,2);
                    UART_PushFrame(usart_send,21);
#else
                    memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);
                    memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);
                    anc_report_num++;
#endif
#ifdef DEBUG_INF
                    printf("序号：%d\r\n",frame_seq_nb2);
#endif
                } else {
                    //   printf("%d",hex_dist);
                    misdist_num[taglist_pos]++;
                }
            }
        }
    } else {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }
}
uint32_t time_monitor[10];
extern uint16_t configremotetagID;
extern u8 remotetag_paralen,userdatasend_flag[TAG_NUM_IN_SYS];
u8 finalsend=0;
u16 signalpower_list[TAG_NUM_IN_SYS];
uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
double Ra, Rb, Da, Db;
int64_t tof_dtu;
u32 errortimes,goodtimes;
u8 tagpossend_flag;
u8 Anchor_RecNearPoll(u8 ancrec_nearbasepos) //0 mainbase  1 first near_base
{

    tmp_time=TIM3->CNT;
    memcpy(&tx_nearresp_msg[NR_NEARSWITCH_DISTANCE],&g_com_map[NEARSWITCH_DISTANCE_INDEX],2);
    memcpy(&tx_nearresp_msg[ANCTIMEMS],&sync_timer,2);
    memcpy(&tx_nearresp_msg[ANCTIMEUS],&tmp_time,2);
    memcpy(&tx_nearresp_msg[TAGSLOTPOS],&taglist_pos,2);
    memcpy(&tx_nearresp_msg[TAG_ID_IDX],&tag_id_recv,2);
    poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2

    resp_tx_time = (poll_rx_ts + ((POLL_RX_TO_RESP_TX_DLY_UUS+(ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS) * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
    dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
    dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS+(rec_nearbase_num+1-ancrec_nearbasepos)*DELAY_BETWEEN_TWO_FRAME_UUS);//设置发送完成后开启接收延迟时间
    dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间
    //dwt_readdiagnostics(&d1);
    if(tagpos[taglist_pos].tagid==tag_id_recv&&userdatasend_flag[taglist_pos] == 1)
    {
        memcpy(&tx_nearresp_msg[TAGPOS_INDEX],&tagpos[taglist_pos],tagpos[taglist_pos].datalen+3);
        tagpossend_flag = 1;
    } else {
        tagpossend_flag = 0;
    }
    if(new_tagid)
    {
        tagdist_list[taglist_pos]=0x1ffff;
    }
    memcpy(&tx_nearresp_msg[DIST_IDX], &tagdist_list[taglist_pos], 4);
    memcpy(&tx_nearresp_msg[ANC_SIGNALPOWER],&signalpower_list[taglist_pos],2);
    tx_nearresp_msg[MAINBASE_INDEX]=flag_syncbase;
    tx_nearresp_msg[MESSAGE_TYPE_IDX]=POS_RESPONSE;
    //tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
    waittagconfig_reponse=0;
    finalsend = 0;
    if(pwtag.remain_time>0)
    {   uint8_t i;
        for(i=0; i<pwtag.groupnum; i++)
        {
            if(pwtag.groupid[i][0]<=tag_id_recv&&tag_id_recv<=pwtag.groupid[i][1])
            {
                remotesend_state=1;
                tx_nearresp_msg[MOTORSTATE_INDEX]=(remotesend_state<<4)|motorstate;
                remotetag_paralen = 2+3;
                remotetag_para[0] = 2;
                remotetag_para[1] = pwtag.index;
                remotetag_para[2] = 2;
                memcpy(&remotetag_para[3],&pwtag.group_interval[i],2);
                memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,remotetag_paralen);
                dwt_writetxdata(24+remotetag_paralen, tx_nearresp_msg, 0);//写入发送数据
                dwt_writetxfctrl(24+remotetag_paralen, 0);//设定发送长度
                remotesend_state=0;
                finalsend = 1;
                break;
            }
        }
    } else {

    }

    if(finalsend == 0)
    {
        memcpy(&tx_nearresp_msg[MAXRANGE_DISTANCE],&g_com_map[MAXRANGDIST_INDEX],2);
        dwt_writetxdata(28+tagpossend_flag*(tagpos[taglist_pos].datalen+3), tx_nearresp_msg, 0);//д????????
        dwt_writetxfctrl(28+tagpossend_flag*(tagpos[taglist_pos].datalen+3), 0);//?څ???????
    }

//          if(remotesend_state&&tag_id_recv==configremotetagID)
//         {
//             memcpy(&tx_nearresp_msg[REMOTEPARA_INDEX],remotetag_para,remotetag_paralen);
//            dwt_writetxdata(22+remotetag_paralen, tx_nearresp_msg, 0);//写入发送数据
//            dwt_writetxfctrl(22+remotetag_paralen, 0);//设定发送长度
//            waittagconfig_reponse=1;
//         }else{

    result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收

    if(result==0)
    {
        while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///不断查询芯片状态直到接收成功或者出现错误
        { };
    } else {
        result++;
    }

    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(seize_anchor&&memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,2)) //抢占anchor å¤±è´¥
        {
            return 1;
        }
        if (rx_buffer[MESSAGE_TYPE_IDX] == POS_FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,2)) //判断是否为Final包
        {

            resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3
            final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6

//               dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
//               dwt_rxenable(0);//打开接收

            final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1，T4，T5
            final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_NEARBASE_IDX+ancrec_nearbasepos*4], &resp_rx_ts);
            final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

            poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
            resp_tx_ts_32 = (uint32_t)resp_tx_ts;
            final_rx_ts_32 = (uint32_t)final_rx_ts;
//               time_monitor[1] = sync_timer*1000+TIM3->CNT;
            Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
            Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
            Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
            Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);//Treply1 = T3 - T2
            tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计算公式
            tof = tof_dtu * DWT_TIME_UNITS;
            distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
            dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数
            dist_cm = dist_no_bias * 100; //dis ä¸ºå•ä½ä¸ºcm的距离
            dwt_readdiagnostics(&d1);
//               time_monitor[2] = sync_timer*1000+TIM3->CNT;
            LOS(&d1);
            //   time_monitor[3] = sync_timer*1000+TIM3->CNT;
            /*--------------------------以下为非测距逻辑------------------------*/
            //dist_cm=33000;
            //GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_RESET);
            LED0_BLINK; //每成功一次通讯则闪烁一次
            dis_after_filter=dist_cm;
            hex_dist = dist_cm+(int16_t)g_com_map[DIST_OFFSET];
            userdatasend_flag[taglist_pos] =0;
            if(hex_dist>-1000&&hex_dist<200000)
            {
                g_flag_Taggetdist[taglist_pos]=0;
                if(abs(hex_dist-his_dist[taglist_pos])<1500||misdist_num[taglist_pos]>3)
                {
                    goodtimes++;
                    misdist_num[taglist_pos]=0;
                    tagdist_list[taglist_pos] = hex_dist;
                    his_dist[taglist_pos]=hex_dist;
                    g_Tagdist[taglist_pos]=hex_dist;
                    signalpower_list[taglist_pos] = ((uint8_t)firstpath_power<<8)|(uint8_t)((rx_power-firstpath_power)*10);
//            #ifndef USART_INTEGRATE_OUTPUT
                    usart_send[2] = 1;//正常模式
                    usart_send[3] = 17;//数据段长度
                    usart_send[4] = frame_seq_nb2;//数据段长度
                    memcpy(&usart_send[5],&tag_id_recv,2);
                    memcpy(&usart_send[7],&dev_id,2);
                    memcpy(&usart_send[9],&tagdist_list[taglist_pos],4);
                    usart_send[13] = battary;
                    usart_send[14] = button;
                    usart_send[15] = firstpath_power;
                    usart_send[16] = (rx_power-firstpath_power)*10;
                    usart_send[17] = tag_frequency;
                    usart_send[18] = tag_slotpos;
                    checksum = Checksum_u16(&usart_send[2],17);
                    memcpy(&usart_send[19],&checksum,2);
                    //UART_PushFrame(usart_send,21);
//            #else
//               memcpy(&usart_send_anc[4+6*anc_report_num],&tag_id_recv,2);
//               memcpy(&usart_send_anc[6+6*anc_report_num],&tagdist_list[taglist_pos],4);
//               anc_report_num++;
//            #endif
//             #ifdef DEBUG_INF
//             printf("序号：%d\r\n",frame_seq_nb2);
//             #endif
                } else {
                    //   printf("%d",hex_dist);
                    misdist_num[taglist_pos]++;
                }
            } else {
                errortimes++;
                usart_send[13] = battary;
            }
            if(rx_buffer[TAGCONFIGSUCCESS_INDEX]==1)
            {

                remotesend_state = 0;
                usart_send[2] = 7;//正常模式
                usart_send[3] = 5;//数据段长度
                memcpy(&usart_send[4],&tag_id_recv,2);
                usart_send[6] = 1;//数据段长度
                checksum = Checksum_u16(&usart_send[2],5);
                memcpy(&usart_send[7],&checksum,2);
                UART_PushFrame(usart_send,9);
            }
        }
    } else {
#ifdef DEBUG_INF
        printf("final包失败：%x/n",status_reg);
#endif
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }
}
void Anchor_Start(void)
{
    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
    g_start_sync_flag=0;

    dwt_setrxtimeout(1000);//设定接收超时时间，0位没有超时时间
    dwt_rxenable(0);//打开接收
//    while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag&&!g_start_sync_flag)//不断查询芯片状态直到接收成功或者出现错误
//   {
//      //IdleTask();
//   };
}
extern int32_t intheight;
extern u16 synclost_count;
uint32_t current_syncid=0xffffffff,synclost_timer;
extern u8 flag_syncbase,waitsync_flag;
u8 tagpos_rec[50],tagpos_send[50],ancidlist_num,zero_matrix[100]= {0};
u16 ancidlist_rec[20],ancidlist_send[20];
u16 recnearbaselist_id[30],rec_taganc_signalpower_list[11];
int32_t recnearbaselist_dist[30];
int16_t rec_tagheight;
u16 timm = 0;
void Anchor_App(void)
{
   uint32_t frame_len;
   uint32_t resp_tx_time;
	
   /* Clear reception timeout to start next ranging process. */
   dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间

   /* Activate reception immediately. */
   dwt_rxenable(0);//打开接收
    u8 send_len,i;
    u16 tempid;
    uint32_t rec_syncid;

   /* Poll for reception of a frame or error/timeout. See NOTE 7 below. */
   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))&&!g_start_send_flag)//不断查询芯片状态直到接收成功或者出现错误
   { 
      UART_CheckReceive();
      UART_CheckSend();
      g_Resttimer=0;
   };
//   dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR| SYS_STATUS_TXFRS |SYS_STATUS_RXFCG);
//   g_start_sync_flag=0;

   if (status_reg & SYS_STATUS_RXFCG)//成功接收
   { u16 tag_recv_interval;
      /* Clear good RX frame event in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位
//   dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
//   dwt_rxenable(0);//打开接收
//   GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_SET);
//   while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))//不断查询芯片状态直到接收成功或者出现错误
//   {
//      //IdleTask();
//   };
//GPIO_WriteBit(GPIOA, GPIO_Pin_10, Bit_RESET);
    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 0);
    status_reg  = dwt_read32bitreg(SYS_STATUS_ID);
    if (status_reg & SYS_STATUS_RXFCG)//成功接收
    {   u16 tag_recv_interval;
        float temp_tagpos;
//   time_monitor[0] = sync_timer*1000+TIM3->CNT;
        g_Resttimer=0;
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除标志位
        frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
        dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据
        memcpy(&anc_id_recv,&rx_buffer[ANCHOR_ID_IDX],2);
        //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
        memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
        switch(rx_buffer[MESSAGE_TYPE_IDX])
        {
            uint16_t checksum;
        case DISCOVERPOLL:
            if (anchor_type == rx_buffer[ANCHOR_ID_IDX]&&g_com_map[NEARSWITCH_DISTANCE_INDEX] != 0)
            {
                taglist_pos=CmpTagInList(tag_id_recv);
                if(taglist_pos==taglist_num)
                {
                    taglist_pos=taglist_num;
                    tagid_list[taglist_num++]=tag_id_recv;

      /* A frame has been received, read it into the local buffer. */
      frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//获得接收数据长度
                    new_tagid=1;
                } else {
                    new_tagid=0;
                }
                tagofflinetime[taglist_pos]=0;

      dwt_readrxdata(rx_buffer, frame_len, 0);//读取接收数据
                rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
                if(rec_nearbase_num>=30)
                {
                    break;
                }
                battary = rx_buffer[BATTARY_IDX];
                button = rx_buffer[BUTTON_IDX];
                frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
                tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
                tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];

                ////////////////基站汇总模式数据
                memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
                memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
                memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4);
                Anchor_RecPoll();
            }
            break;
        case SPOLL:
            //if (anchor_type == rx_buffer[ANC_TYPE_IDX])
            if(dev_id==anc_id_recv)
            {
                taglist_pos=CmpTagInList(tag_id_recv);
                if(taglist_pos==taglist_num)
                {
                    taglist_pos=taglist_num;
                    tagid_list[taglist_num++]=tag_id_recv;

                    new_tagid=1;
                } else {
                    new_tagid=0;
                }
                tagofflinetime[taglist_pos]=0;

                rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
                if(rec_nearbase_num>=30)
                {
                    break;
                }
                battary = rx_buffer[BATTARY_IDX];
                button = rx_buffer[BUTTON_IDX];
                frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
                tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
                tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];

                ////////////////基站汇总模式数据
                memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6],2);
                memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
                memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4);
                usart_send[2] = 0x0c;//正常模式
                usart_send[3] = 15+8*(rec_nearbase_num);//数据段长度
                memcpy(&usart_send[4],&tag_id_recv,2);
                usart_send[6] = rx_buffer[SEQUENCE_IDX];
                usart_send[7] = rx_buffer[SEQUENCEH_IDX];
                usart_send[8] = battary;
                usart_send[9] = button;
                memcpy(&usart_send[10],&rec_tagheight,2);
                usart_send[12] = tag_frequency;
                usart_send[13] = tag_slotpos;
                usart_send[14] = 0;
                usart_send[15] = 0;
                usart_send[16] = rec_nearbase_num;

                memcpy(&usart_send[17],&recnearbaselist_id,2*rec_nearbase_num);
                memcpy(&usart_send[17+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num);
                memcpy(&usart_send[17+rec_nearbase_num*6],zero_matrix,2*rec_nearbase_num);

                checksum = Checksum_u16(&usart_send[2],15+8*rec_nearbase_num);
                memcpy(&usart_send[17+8*rec_nearbase_num],&checksum,2);
                UART_PushFrame(usart_send,19+8*rec_nearbase_num);
                Anchor_RecPoll();
            }
            break;
        case SYNC:
            memcpy(&rec_syncid,&rx_buffer[ANCHOR_ID_IDX],4);
            if(anc_id_recv==g_com_map[SYNCBASEID])
            {
                synclost_count = 0;
                flag_syncbase=0;
                sync_seq=rx_buffer[SYNC_SEQ_IDX]+1;
                TIM3->CNT = (sync_seq*325%1000)-27;
                sync_timer = (sync_seq*325/1000);
                synclost_timer=0;
                if(g_com_map[BASESYNCSEQ]==2)
                    SyncPoll(sync_seq);
            }
            break;
        case POS_MSG:
            if(anc_id_recv==dev_id)
            {
                rx_buffer[TAGSLOTPOS]=taglist_pos;
                tx_near_msg[MESSAGE_TYPE_IDX] = POS_MSG;
                memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
                memcpy(&tx_near_msg[NEARMSG_EMPTYSLOTPOS_INDEX],&tagpos_binary,4);
                ancidlist_num=g_com_map[NEARBASE_NUM];
                tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
                memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
                send_len=21+ancidlist_num*2;
                dwt_writetxdata(send_len, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
                dwt_writetxfctrl(send_len, 0);//设置超宽带发送数据长度
                dwt_starttx(DWT_START_TX_IMMEDIATE);
            }
            break;
        case REG_POLL:
            for(i=0; i<MAX_REGTAGNUM; i++)
            {
                if(tag_id_recv==regtag_map.tagid[i]&&regtag_map.remain_time[i]>0)
                {
                    tx_near_msg[REGR_TAGSLOTPOS_INDEX]=regtag_map.tag_slotpos[i];
                    tx_near_msg[REGR_TAGFREQ_INDEX] = regtag_map.tag_frequency[i];
                    memcpy(&tx_near_msg[TAG_ID_IDX],&tag_id_recv,2);
                    memcpy(&tx_near_msg[ANCHOR_ID_IDX],&dev_id,2);
                    tx_near_msg[MESSAGE_TYPE_IDX] = REG_RESPONSE;
                    ancidlist_num=g_com_map[NEARBASE_NUM];
                    tx_near_msg[NEARBASENUM_INDEX]=g_com_map[NEARBASE_NUM];
                    memcpy(&tx_near_msg[NEARBASEID_INDEX],&g_com_map[NEARBASE_ID1],g_com_map[NEARBASE_NUM]*2);
                    send_len=21+ancidlist_num*2;
                    dwt_writetxdata(send_len, tx_near_msg, 0);//将Poll包数据传给DW1000，将在开启发送时传出去
                    dwt_writetxfctrl(send_len, 0);//设置超宽带发送数据长度
                    dwt_starttx(DWT_START_TX_IMMEDIATE);
                    regtag_map.tagid[i] = 0;
                    break;
                }

            }
            if(i==MAX_REGTAGNUM&&g_com_map[NEARSWITCH_DISTANCE_INDEX] != 0)
            {
                dwt_readdiagnostics(&d1);
                LOS(&d1);
                usart_send[2]=0x0a;
                usart_send[3]=20;
                memcpy(&usart_send[4],&dev_id,2);
                memcpy(&usart_send[6],&tag_id_recv,2);
                memcpy(&usart_send[8],&rx_buffer[10],9);
                usart_send[17] = firstpath_power;
                memcpy(&usart_send[18],&rx_buffer[19],2);
                checksum = Checksum_u16(&usart_send[2],20);
                memcpy(&usart_send[22],&checksum,2);
                UART_PushFrame(usart_send,24);
            }
            break;
        case POS_POLL:
            //   GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);

            memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],2);
            taglist_pos=CmpTagInList(tag_id_recv);
            if(taglist_pos==taglist_num)
            {
                taglist_pos=taglist_num;
                tagid_list[taglist_num++]=tag_id_recv;

                new_tagid=1;
            } else {
                new_tagid=0;
            }
            tagofflinetime[taglist_pos]=0;
            temp_tagpos=round((float)(sync_timer%g_com_map[COM_INTERVAL])/slottime);
            tagpos_rec[(u8)temp_tagpos]=1;
            rec_nearbase_num=rx_buffer[NEARBASENUM_INDEX];
            if(rec_nearbase_num>30)
            {
                break;
            }
            battary = rx_buffer[BATTARY_IDX];
            button = rx_buffer[BUTTON_IDX];
            frame_seq_nb2 = rx_buffer[SEQUENCE_IDX];
            tag_frequency = rx_buffer[NEARP_TAGFREQ_INDEX];
            tag_slotpos = rx_buffer[NEARP_TAGSLOTPOS_INDEX];
            ////////////////基站汇总模式数据
            memcpy(&rec_tagheight,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*8+6],2);
            memcpy(&recnearbaselist_id,&rx_buffer[NEARBASEID_INDEX],rec_nearbase_num*2);
            memcpy(&recnearbaselist_dist,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*2],rec_nearbase_num*4+4);
            memcpy(&rec_taganc_signalpower_list,&rx_buffer[NEARBASEID_INDEX+rec_nearbase_num*6+4],rec_nearbase_num*2+2);
            if(anc_id_recv == dev_id||g_com_map[OUTPUTALLNEARPOLLMSG] == 1 )
            {
                usart_send[2] = 0x0c;//正常模式
                usart_send[3] = 15+8*(rec_nearbase_num+1);//数据段长度
                memcpy(&usart_send[4],&tag_id_recv,2);
                usart_send[6] = rx_buffer[SEQUENCE_IDX];
                usart_send[7] = rx_buffer[SEQUENCEH_IDX];
                usart_send[8] = battary;
                usart_send[9] = button;

                memcpy(&usart_send[10],&rec_tagheight,2);
                usart_send[12] = tag_frequency;
                usart_send[13] = tag_slotpos;
                usart_send[14] = 0;
                usart_send[15] = 0;
                usart_send[16] = rec_nearbase_num+1;
                memcpy(&usart_send[17],&anc_id_recv,2);
                memcpy(&usart_send[19],&recnearbaselist_id,2*rec_nearbase_num);
                memcpy(&usart_send[19+rec_nearbase_num*2],&recnearbaselist_dist,4*rec_nearbase_num+4);
                memcpy(&usart_send[19+rec_nearbase_num*6+4],&rec_taganc_signalpower_list,2*rec_nearbase_num+2);

                checksum = Checksum_u16(&usart_send[2],23+8*rec_nearbase_num);
                memcpy(&usart_send[25+8*rec_nearbase_num],&checksum,2);
                UART_PushFrame(usart_send,27+8*rec_nearbase_num);
//               UART_CheckSend();

                timm++;
            }
            //////////////////////////////////////////////////////////////


      /* Check that the frame is a poll sent by "DS TWR initiator" example.
       * As the sequence number field of the frame is not relevant, it is cleared to simplify the validation of the frame. */
	
		
      //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
      //tag_id_recv = rx_buffer[TAG_ID_IDX];
      memcpy(&tag_id_recv,&rx_buffer[TAG_ID_IDX],4);
      memcpy(&tx_resp_msg[TAG_ID_IDX],&tag_id_recv,4);
      //tx_resp_msg[TAG_ID_IDX] = tag_id_recv;
//      if(tag_recv_timer>tag_time_recv[tag_id_recv-TAG_ID_START])
//      {   tag_recv_interval =  tag_recv_timer - tag_time_recv[tag_id_recv];
//      }else{
//         tag_recv_interval = tag_recv_timer + 65535 - tag_time_recv[tag_id_recv];
//      }
		
      if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&(anchor_type == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      {
         /* Retrieve poll reception timestamp. */
         poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
            GPIO_WriteBit(GPIOB, GPIO_Pin_10, Bit_SET);
            if(anc_id_recv==dev_id)
            {
                Anchor_RecNearPoll(rec_nearbase_num);
            } else {

         /* Set send time for response. See NOTE 8 below. */
         resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算Response发送时间T3。
         dwt_setdelayedtrxtime(resp_tx_time);//设置Response发送时间T3
                for(i=0; i<rec_nearbase_num; i++)
                {
                    memcpy(&tempid,&rx_buffer[NEARBASEID_INDEX+i*2],2);
                    if(tempid==dev_id)
                    {

         /* Set expected delay and timeout for final message reception. */
         dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);//设置发送完成后开启接收延迟时间
         dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间

         /* Write and send the response message. See NOTE 9 below.*/
         if(tag_id_recv-TAG_ID_START<=TOTAL_TAG_NUM)
         memcpy(&tx_resp_msg[DIST_IDX], &anchor_dist_last_frm[tag_id_recv-TAG_ID_START], 4);
		
         dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
         result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收

         battary = rx_buffer[BATTARY_IDX];
         button = rx_buffer[BUTTON_IDX];
         /* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
          * See NOTE 7 below. */
         if(result==0)
         {
            while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///不断查询芯片状态直到接收成功或者出现错误
         { };
      }
         /* Increment frame sequence number after transmission of the response message (modulo 256). */

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

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

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


            /* Check that the frame is a final message sent by "DS TWR initiator" example.
             * As the sequence number field of the frame is not used in this example, it can be zeroed to ease the validation of the frame. */
			
            if (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&!memcmp(&rx_buffer[TAG_ID_IDX],&tag_id_recv,4)&&!memcmp(&rx_buffer[ANCHOR_ID_IDX],&dev_id,4)) //判断是否为Final包
            {
               uint32_t poll_tx_ts, resp_rx_ts, final_tx_ts;
               uint32_t poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
               double Ra, Rb, Da, Db;
               int64_t tof_dtu;
					
               /* Retrieve response transmission and final reception timestamps. */
               resp_tx_ts = get_tx_timestamp_u64();//获得response发送时间T3
               final_rx_ts = get_rx_timestamp_u64();//获得final接收时间T6

               /* Get timestamps embedded in the final message. */
               final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从接收数据中读取T1，T4，T5
               final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
               final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

               /* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
               poll_rx_ts_32 = (uint32_t)poll_rx_ts;//使用32位数据计算
               resp_tx_ts_32 = (uint32_t)resp_tx_ts;
               final_rx_ts_32 = (uint32_t)final_rx_ts;
               Ra = (double)(resp_rx_ts - poll_tx_ts);//Tround1 = T4 - T1
               Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);//Tround2 = T6 - T3
               Da = (double)(final_tx_ts - resp_rx_ts);//Treply2 = T5 - T4
               Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);//Treply1 = T3 - T2
               tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计算公式

               tof = tof_dtu * DWT_TIME_UNITS;
               distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
               dist_no_bias = distance - dwt_getrangebias(config.chan, (float)distance, config.prf); //距离减去矫正系数

               dist_cm = dist_no_bias * 100; //dis ä¸ºå•ä½ä¸ºcm的距离
//               dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½Žé€šæ»¤æ³¢å™¨ï¼Œè®©æ•°æ®æ›´ç¨³å®š
					
               /*--------------------------以下为非测距逻辑------------------------*/
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
      //         g_Tagdist[tag_id_recv-TAG_ID_START]=dist_cm;
//               if(g_pairstart==1&&dist_cm<20)
                        seize_anchor=0;  //非抢占。已存在列表中
                        Anchor_RecNearPoll(i);
                        break;
                    }
                }
            }
            //抢占模式
//               if(i==rec_nearbase_num)
//               {
//                  g_pairstart=0;
//                  g_com_map[PAIR_ID]=tag_id_recv;
//                  save_com_map_to_flash();
//                  BEEP2_ON;
//                  delay_ms(1000);
//                  printf("Pair Finish PairID: %d. \r\n",g_com_map[PAIR_ID]);
//                  seize_anchor=1;   //抢占anchor
//                  Anchor_RecNearPoll(i);
//               }
               // tag_time_recv[tag_id_recv] = tag_recv_timer;
                  if(tag_id_recv-TAG_ID_START<=TOTAL_TAG_NUM)
                  {
               g_flag_Taggetdist[tag_id_recv-TAG_ID_START]=0;
               anchor_dist_last_frm[tag_id_recv-TAG_ID_START] = dist_cm;
                  }
               #ifdef HEX_OUTPUT
               usart_send[2] = frame_seq_nb++;
               //usart_send[6] = tag_id_recv;
               //usart_send[8] = g_com_map[DEV_ID];
               memcpy(&usart_send[3],&tag_id_recv,4);
               memcpy(&usart_send[7],&dev_id,4);
               hex_dist = dist_cm;
               memcpy(&usart_send[11],&hex_dist,4);
               usart_send[15] = battary;
               usart_send[16] = button;
               checksum = Checksum_u16(&usart_send[2],19);
               memcpy(&usart_send[21],&checksum,2);
               UART_PushFrame(usart_send,23);
               #else
               printf("Anchor ID: %d, Tag ID: %d, Dist = %d cm\n", g_com_map[DEV_ID_L]|g_com_map[DEV_ID_L]<<8, tag_id_recv, (uint16_t)dist_cm);
               #endif
					
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
//                    if(tag_id_recv==0x4008)
//                    printf("标签高度: %d,基站高度: %d,高度差： %d. \r\n",rec_tagheight,intheight,intheight-rec_tagheight);

            }
         }else{
            /* Clear RX error events in the DW1000 status register. */
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
         }
      }
   }
   else
   {
      /* Clear RX error events in the DW1000 status register. */
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
   }
            break;
        default:

            break;
        }
    } else {
#ifdef DEBUG_INF
        printf("poll包失败：%x/n",status_reg);
#endif
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
    }
    UART_CheckSend();
    dwt_setinterrupt(  DWT_INT_RFCG | (DWT_INT_ARFE | DWT_INT_RFSL | DWT_INT_SFDT | DWT_INT_RPHE | DWT_INT_RFCE | DWT_INT_RFTO | DWT_INT_RXPTO), 1);
    dwt_setrxtimeout(0);//设定接收超时时间，0位没有超时时间
    dwt_rxenable(0);//打开接收

}