西安结束

zhyinch

2019-06-10 ba39ac4c74b834cc69ac60a8117fc9aa0a3ff1d3

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

@@ -30,7 +30,8 @@
#include "Usart.h"
#include "global_param.h"
#include "filters.h"

#include <stdio.h>
#include "beep.h"


/*------------------------------------ Marcos ------------------------------------------*/
@@ -74,6 +75,7 @@
#define TAG_ID_IDX                3
#define MESSAGE_TYPE_IDX          5
#define DIST_IDX                      6
#define ANC_TYPE_IDX                7

#define POLL                    0x01
#define RESPONSE                0x02
@@ -82,7 +84,7 @@
/*------------------------------------ Variables ------------------------------------------*/
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
static dwt_config_t config = {
   5,               /* Channel number. */
   2,               /* Channel number. */
   DWT_PRF_64M,     /* Pulse repetition frequency. */
   DWT_PLEN_128,    /* Preamble length. */
   DWT_PAC8,        /* Preamble acquisition chunk size. Used in RX only. */
@@ -138,6 +140,8 @@
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器

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


@@ -219,7 +223,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.
@@ -245,6 +265,7 @@
}
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;
@@ -258,31 +279,48 @@
   dwt_configuresleep(0x940, 0x7);
   dwt_entersleep();
}

uint16_t g_Resttimer;
uint8_t result;
u8 tag_succ_times=0;
void Tag_App(void)//发送模式(TAG标签)
{
   uint32_t frame_len;
   uint32_t final_tx_time;
   u32 start_poll;
   u8 i;
   
   g_Resttimer=0;
   UART_CheckReceive();
   GPIO_ResetBits(SPIx_GPIO, SPIx_CS);
   delay_us(2000);
   delay_us(2500);
   GPIO_SetBits(SPIx_GPIO, SPIx_CS);
	
   tag_succ_times = 0;
   for(i=0;i<REPOET_ANC_NUM;i++)
   {
   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */

   tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
   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)))//不断查询芯片状态直到成功接收或者发生错误
   { };
   { if(time32_incr - start_poll>20)
      NVIC_SystemReset();
	
   };

   /* Increment frame sequence number after transmission of the poll message (modulo 256). */
   frame_seq_nb++;
   if(status_reg==0xffffffff)
   {
      NVIC_SystemReset();
   }

   if (status_reg & SYS_STATUS_RXFCG)//如果成功接收
   {
@@ -297,8 +335,8 @@

      /* 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) //判断接收到的数据是否是response数据
      rx_buffer[ALL_MSG_SN_IDX] = 0;
      if (rx_buffer[MESSAGE_TYPE_IDX] == RESPONSE&&rx_buffer[TAG_ID_IDX]==g_com_map[DEV_ID]) //判断接收到的数据是否是response数据
      {
         /* Retrieve poll transmission and response reception timestamp. */
         poll_tx_ts = get_tx_timestamp_u64();                              //获得POLL发送时间T1
@@ -319,20 +357,23 @@
         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. */
		
         tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
         dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//将发送数据写入DW1000
         dwt_writetxfctrl(sizeof(tx_final_msg), 0);//设定发送数据长度
         dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
			
         tag_succ_times++;
         
         /* Poll DW1000 until TX frame sent event set. See NOTE 8 below. */
         while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))//不断查询芯片状态直到发送完成
         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). */
         frame_seq_nb++;
         frame_seq_nb++;		
         random_delay_tim = 0;
      }
      else
@@ -346,12 +387,33 @@
      dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
      random_delay_tim = DFT_RAND_DLY_TIM_MS;
   }
//   deca_sleep(10);
}
   if(tag_succ_times!=REPOET_ANC_NUM)
   {
   random_delay_tim =g_com_map[DEV_ID]*13+7;
   }else{
      random_delay_tim=0;
   }
   LED0_BLINK;
   deca_sleep(random_delay_tim);
   RTC_SET_ALARM(1);
   /* Execute a delay between ranging exchanges. */
   dwt_entersleep();
	
}
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[TOTAL_TAG_NUM];
u8 usart_send[16];
extern uint8_t g_pairstart;
void Anchor_App(void)
{
   uint32_t frame_len;
@@ -368,10 +430,11 @@
   { 
      UART_CheckReceive();
      UART_CheckSend();
      g_Resttimer=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);//清除标志位

@@ -383,14 +446,18 @@

      /* 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. */
	
      rx_buffer[ALL_MSG_SN_IDX] = 0;
      
      //将收到的tag_id分别写入各次通讯的包中，为多标签通讯服务，防止一次通讯中接收到不同ID标签的数据
      tag_id_recv = rx_buffer[TAG_ID_IDX];
      tx_resp_msg[TAG_ID_IDX] = tag_id_recv;

      if(tag_recv_timer>tag_time_recv[tag_id_recv])
      {   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) //判断是否是poll包数据
      if (rx_buffer[MESSAGE_TYPE_IDX] == POLL&&tag_id_recv!= g_com_map[PAIR_ID]&&(tag_recv_interval>g_com_map[COM_INTERVAL]/2)&&(g_com_map[DEV_ID]%REPOET_ANC_NUM == rx_buffer[ANC_TYPE_IDX])) //判断是否是poll包数据
      {
         /* Retrieve poll reception timestamp. */
         poll_rx_ts = get_rx_timestamp_u64();//获得Poll包接收时间T2
@@ -405,16 +472,18 @@

         /* Write and send the response message. See NOTE 9 below.*/
         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);//设定发送长度
         dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收
         result = dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//延迟发送，等待接收

         /* We assume that the transmission is achieved correctly, now poll for reception of expected "final" frame or error/timeout.
          * See NOTE 7 below. */
         while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR)))///不断查询芯片状态直到接收成功或者出现错误
         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). */
         frame_seq_nb++;

@@ -431,13 +500,15 @@

            /* 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[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;
               double Ra, Rb, Da, Db;
               int64_t tof_dtu;

               u32 hex_dist;
               u16 checksum;
               /* 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
@@ -468,12 +539,34 @@
               LED0_BLINK; //每成功一次通讯则闪烁一次
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
               g_Tagdist[tag_id_recv]=dist_cm;
//               if(g_pairstart==1&&dist_cm<20)
//               {
//                  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]);
//               }
                tag_time_recv[tag_id_recv] = tag_recv_timer;
               g_flag_Taggetdist[tag_id_recv]=0;
               #ifdef HEX_OUTPUT
               usart_send[2] = frame_seq_nb;
               usart_send[6] = tag_id_recv;
               usart_send[8] = g_com_map[DEV_ID];
               hex_dist = dist_cm;
               memcpy(&usart_send[10],&hex_dist,4);
               checksum = Checksum_u16(&usart_send[2],12);
               memcpy(&usart_send[14],&checksum,2);
               UART_PushFrame(usart_send,16);
               #else
               printf("Anchor ID: %d, Tag ID: %d, Dist = %d cm\n", g_com_map[DEV_ID], tag_id_recv, (uint16_t)dis_after_filter);
               #endif
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);

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