增加多标签测距功能，标签有bug，长时间会死机

zhyinch

2018-09-28 1ebb084c4d5216f4e5e3e61a63c88f568caf779e

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

@@ -32,6 +32,7 @@
#include "filters.h"



/*------------------------------------ Marcos ------------------------------------------*/
/* Inter-ranging delay period, in milliseconds. */
#define RNG_DELAY_MS 100
@@ -69,8 +70,10 @@
#define FINAL_MSG_TS_LEN 4

#define GROUP_ID_IDX               0
#define SOURCE_ID_IDX             1
#define MESSAGE_TYPE_IDX          3
#define ANCHOR_ID_IDX             1
#define TAG_ID_IDX                3
#define MESSAGE_TYPE_IDX          5
#define DIST_IDX                      6

#define POLL                    0x01
#define RESPONSE                0x02
@@ -135,6 +138,8 @@
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器

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


@@ -216,7 +221,23 @@
        *ts += ts_field[i] << (i * 8);
    }
}

void TagDistClear(void)
{
   static uint16_t clear_judge_cnt;
   uint16_t i;
   if(clear_judge_cnt++>1000)  //设定1S分频，每秒进一次。判断标志位大于等于2，2s没收到数据就把数据变成0xffff，不触发警报。
   {
      clear_judge_cnt=0;
      for(i=0;i<255;i++)
      {
         g_flag_Taggetdist[i]++;
         if(g_flag_Taggetdist[i]>=2)
         {
            g_Tagdist[i]=0xffff;
         }
      }
   }
}
void Dw1000_Init(void)
{
   /* Reset and initialise DW1000.
@@ -240,7 +261,17 @@
    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]=0x03;
   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], &g_com_map[DEV_ID], 2);
   memcpy(&tx_final_msg[TAG_ID_IDX], &g_com_map[DEV_ID], 2);
   memcpy(&tx_resp_msg[ANCHOR_ID_IDX], &g_com_map[DEV_ID], 2);
	
}	
void tag_sleep_configuraion(void)
{
   dwt_configuresleep(0x940, 0x7);
@@ -286,14 +317,14 @@
      /* 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. */
      rx_buffer[ALL_MSG_SN_IDX] = 0;
      if (rx_buffer[9] == 0x10) //判断接收到的数据是否是response数据
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE) //判断接收到的数据是否是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[11], 2);

			
         memcpy(&anchor_dist_last_frm[tag_id], &rx_buffer[DIST_IDX], 2);
         memcpy(&tx_final_msg[ANCHOR_ID_IDX], &rx_buffer[ANCHOR_ID_IDX], 2);
         /* 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
@@ -374,11 +405,11 @@
      rx_buffer[ALL_MSG_SN_IDX] = 0;
      
      //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
      tag_id_recv = rx_buffer[5];
      tx_resp_msg[5] = tag_id_recv;
      tag_id_recv = rx_buffer[TAG_ID_IDX];
      tx_resp_msg[TAG_ID_IDX] = tag_id_recv;

      
      if (rx_buffer[9] == 0x21) //判断是否是poll包数据
      if (rx_buffer[MESSAGE_TYPE_IDX] == POLL) //判断是否是poll包数据
      {
         /* Retrieve poll reception timestamp. */
         poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
@@ -392,7 +423,7 @@
         dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//接收超时时间

         /* Write and send the response message. See NOTE 9 below.*/
         memcpy(&tx_resp_msg[11], &anchor_dist_last_frm[tag_id_recv], 2);
         memcpy(&tx_resp_msg[DIST_IDX], &anchor_dist_last_frm[tag_id_recv], 2);
         tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
         dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//写入发送数据
         dwt_writetxfctrl(sizeof(tx_resp_msg), 0);//设定发送长度
@@ -420,7 +451,7 @@
            /* 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. */
            rx_buffer[ALL_MSG_SN_IDX] = 0;
            if (rx_buffer[9] == 0x23) //判断是否为Final包
            if (rx_buffer[MESSAGE_TYPE_IDX] == FINAL&&rx_buffer[TAG_ID_IDX]==tag_id_recv&&rx_buffer[ANCHOR_ID_IDX]==g_com_map[DEV_ID]) //判断是否为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;
@@ -456,7 +487,10 @@
               /*--------------------------以下为非测距逻辑------------------------*/
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
               dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
               dis_after_filter=dist_cm;
               g_Tagdist[tag_id_recv]=dist_cm;
               g_flag_Taggetdist[tag_id_recv]=0;
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);

            }
         }