guanjiao
2018-10-07 dca9d1109b42085df12402d6b54006a23a9aaa75
Ô´Âë/ºËÐİå/Src/application/dw_app.c
@@ -26,6 +26,12 @@
#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>
/*------------------------------------ Marcos ------------------------------------------*/
@@ -45,13 +51,13 @@
#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 4100
#define RESP_RX_TO_FINAL_TX_DLY_UUS 1500
/* Receive response timeout. See NOTE 5 below. */
#define RESP_RX_TIMEOUT_UUS 14700
#define RESP_RX_TIMEOUT_UUS 2700
#define POLL_RX_TO_RESP_TX_DLY_UUS 3600
#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 500
#define RESP_TX_TO_FINAL_RX_DLY_UUS 200
/* Receive final timeout. See NOTE 5 below. */
#define FINAL_RX_TIMEOUT_UUS 4300
@@ -65,8 +71,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
@@ -74,52 +82,49 @@
/*------------------------------------ Variables ------------------------------------------*/
/* Default communication configuration. We use here EVK1000's default mode (mode 3). */
static dwt_config_t config =
{
    2,               /* Channel number. */
    DWT_PRF_64M,     /* Pulse repetition frequency. */
    DWT_PLEN_1024,   /* Preamble length. */
    DWT_PAC32,       /* 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_110K,     /* Data rate. */
    DWT_PHRMODE_STD, /* PHY header mode. */
    (1025 + 64 - 32) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
static dwt_config_t config = {
   5,               /* 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. */
};
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 tx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
//static uint8 rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8 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 tx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 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 rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8 tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
//static uint8 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};
//static uint8_t rx_poll_msg[] = {0x00, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8_t tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 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 frame_seq_nb = 0;
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 uint32 status_reg = 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 rx_buffer[RX_BUF_LEN];
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. */
typedef unsigned long long uint64;
static uint64 poll_tx_ts;
static uint64 resp_rx_ts;
static uint64 final_tx_ts;
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). */
typedef signed long long int64;
static uint64 poll_rx_ts;
static uint64 resp_tx_ts;
static uint64 final_rx_ts;
static uint64_t poll_rx_ts;
static uint64_t resp_tx_ts;
static uint64_t final_rx_ts;
static double tof;
   
@@ -130,6 +135,12 @@
double distance, dist_no_bias, dist_cm;
uint32_t g_UWB_com_interval = 0;
float dis_after_filter;            //当前距离值
LPFilter_Frac* p_Dis_Filter;      //测距用的低通滤波器
uint16_t g_Tagdist[256];
uint8_t g_flag_Taggetdist[256];
/*------------------------------------ Functions ------------------------------------------*/
@@ -143,10 +154,10 @@
 *
 * @return  64-bit value of the read time-stamp.
 */
static uint64 get_tx_timestamp_u64(void)
static uint64_t get_tx_timestamp_u64(void)
{
    uint8 ts_tab[5];
    uint64 ts = 0;
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readtxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
@@ -167,10 +178,10 @@
 *
 * @return  64-bit value of the read time-stamp.
 */
static uint64 get_rx_timestamp_u64(void)
static uint64_t get_rx_timestamp_u64(void)
{
    uint8 ts_tab[5];
    uint64 ts = 0;
    uint8_t ts_tab[5];
    uint64_t ts = 0;
    int i;
    dwt_readrxtimestamp(ts_tab);
    for (i = 4; i >= 0; i--)
@@ -192,17 +203,17 @@
 *
 * @return none
 */
static void final_msg_set_ts(uint8 *ts_field, uint64 ts)
static void final_msg_set_ts(uint8_t *ts_field, uint64_t ts)
{
    int i;
    for (i = 0; i < FINAL_MSG_TS_LEN; i++)
    {
        ts_field[i] = (uint8) ts;
        ts_field[i] = (uint8_t) ts;
        ts >>= 8;
    }
}
static void final_msg_get_ts(const uint8 *ts_field, uint32 *ts)
static void final_msg_get_ts(const uint8_t *ts_field, uint32_t *ts)
{
    int i;
    *ts = 0;
@@ -211,7 +222,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.
@@ -223,7 +250,9 @@
    /* Configure DW1000. See NOTE 6 below. */
    dwt_configure(&config);//配置DW1000
    /* Apply default antenna delay value. See NOTE 1 below. */
    dwt_setrxantennadelay(RX_ANT_DLY);      //设置接收天线延迟
    dwt_settxantennadelay(TX_ANT_DLY);      //设置发射天线延迟
@@ -233,11 +262,31 @@
    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] = 0x04;
   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);
   dwt_entersleep();
}
uint8_t result;
void Tag_App(void)//发送模式(TAG标签)
{
   uint32 frame_len;
   uint32 final_tx_time;
   uint32_t frame_len;
   uint32_t final_tx_time;
   GPIO_ResetBits(SPIx_GPIO, SPIx_CS);
   delay_us(2500);
   GPIO_SetBits(SPIx_GPIO, SPIx_CS);
   
   /* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
   tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
@@ -269,20 +318,20 @@
      /* 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
         /* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
         final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;//final包实际发送时间是计算时间加上发送天线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写入发送数据
@@ -293,32 +342,14 @@
         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);//设定为延迟发送
         //这里为串口输出
//         if (GPIO_ReadInputDataBit(GPIOA, SW2) != RESET) //通过拨码开关判断数据输出格式
//         {
//            dID = TAG_ID;
//            printf("TAG_ID: %2.0f      ", dID);
//            dID = ANCHOR_ID;
//            printf("ANCHOR_ID: %2.0f      ", dID);
//            printf("Distance: %5.0f cm\n", (double)dist[TAG_ID]);
//         }
//         else
//         {
//            send[2] = ANCHOR_ID;
//            send[3] = TAG_ID;
//            memcpy(&send[4], &dist[TAG_ID], 2);
//            check = Checksum_u16(&send[2], 6);
//            memcpy(&send[8], &check, 2);
//            USART_puts(send, 10);
//         }
         result=dwt_starttx(DWT_START_TX_DELAYED);//设定为延迟发送
         
         /* 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);//清除标志位
@@ -339,13 +370,14 @@
   }
   LED0_BLINK;
   /* Execute a delay between ranging exchanges. */
   deca_sleep(RNG_DELAY_MS + random_delay_tim); //休眠固定时间
   dwt_entersleep();
}
uint16_t g_Resttimer;
void Anchor_App(void)
{
   uint32 frame_len;
   uint32 resp_tx_time;
   uint32_t frame_len;
   uint32_t resp_tx_time;
   
   /* Clear reception timeout to start next ranging process. */
   dwt_setrxtimeout(0);//设定接收超时时间,0位没有超时时间
@@ -355,7 +387,11 @@
   /* 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)))//不断查询芯片状态直到接收成功或者出现错误
   { };
   {
      UART_CheckReceive();
      UART_CheckSend();
      g_Resttimer=0;
   };
   if (status_reg & SYS_STATUS_RXFCG)//成功接收
   {
@@ -373,11 +409,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
@@ -391,17 +427,19 @@
         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);//设定发送长度
         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++;
@@ -419,10 +457,10 @@
            /* 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 poll_tx_ts, resp_rx_ts, final_tx_ts;
               uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
               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;
@@ -436,14 +474,14 @@
               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)poll_rx_ts;//使用32位数据计算
               resp_tx_ts_32 = (uint32)resp_tx_ts;
               final_rx_ts_32 = (uint32)final_rx_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)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计算公式
               tof_dtu = (int64_t)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));//计算公式
               tof = tof_dtu * DWT_TIME_UNITS;
               distance = tof * SPEED_OF_LIGHT;//距离=光速*飞行时间
@@ -452,26 +490,14 @@
               dist_cm = dist_no_bias * 100; //dis ä¸ºå•位为cm的距离
//               dist[TAG_ID] = LP(dis, TAG_ID); //LP ä¸ºä½Žé€šæ»¤æ³¢å™¨ï¼Œè®©æ•°æ®æ›´ç¨³å®š
               
               /*--------------------------以下为非测距逻辑------------------------*/
               LED0_BLINK; //每成功一次通讯则闪烁一次
               //这里供串口输出
//               if (GPIO_ReadInputDataBit(GPIOA, SW2) != RESET) //通过拨码开关判断数据输出格式
//               {
//                  dID = TAG_ID;
//                  printf("TAG_ID: %2.0f      ", dID);
//                  dID = ANCHOR_ID;
//                  printf("ANCHOR_ID: %2.0f      ", dID);
//                  printf("Distance: %5.0f cm\n", (double)dist[TAG_ID]);
//               }
//               else
//               {
//                  send[2] = ANCHOR_ID;
//                  send[3] = TAG_ID;
//                  memcpy(&send[4], &dist[TAG_ID], 2);
//                  check = Checksum_u16(&send[2], 6);
//                  memcpy(&send[8], &check, 2);
//                  USART_puts(send, 10);
//               }
               g_UWB_com_interval = 0;
               dis_after_filter=dist_cm;
               g_Tagdist[tag_id_recv]=dist_cm;
               g_flag_Taggetdist[tag_id_recv]=0;
               printf("Anchor ID: %d, Tag ID: %d, Dist = %d cm\n", g_com_map[DEV_ID], tag_id_recv, (uint16_t)dis_after_filter);
               //dis_after_filter = LP_Frac_Update(p_Dis_Filter, dist_cm);
            }
         }