UWB DWM1000 開源項目框架 之 TWR測距實現

TWR測距方法是目前推廣的一種測距方法，也是目前大多數用戶使用的測距方法，具體原理如下圖

具體算法參見:DW1000 用戶手冊中文版 附錄3：雙向測距(Two-Way Ranging)

原理很簡單，節點A發送信息Poll給節點B，節點B應答Responese，節點A收到應答再回復一條Final信息。

在我們框架里實現代碼也是非常容易 

A 第一個信息Poll的代碼

發送給節點B，我們測試的時候，節點B地址=節點A地址+1

發送的數據包，第一個個信息"P"指明了是一個Poll信息

    msg_f_send.destAddr[0] =(SHORT_ADDR+1) &0xFF;
    msg_f_send.destAddr[1] =  ((SHORT_ADDR+1)>>8) &0xFF;

    msg_f_send.seqNum = distance_seqnum;
    msg_f_send.messageData[0]='P';//Poll message
    //后面修改這個數據長度
    dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
    dwt_writetxfctrl(psduLength, 0);
    dwt_starttx(DWT_START_TX_IMMEDIATE);
    while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
    { };

    dwt_enableframefilter(DWT_FF_DATA_EN);
    dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*10);
    dwt_rxenable(0);

    /* Clear good RX frame event in the DW1000 status register. */
    if(++distance_seqnum == 255)
        distance_seqnum = 0;
    Tag_State = TAG_POLL_SENT;

B 節點B收到Poll信息，並返回Responese 信息

節點B接到Poll信息的時候，通過get_rx_timestamp_u64()讀取了poll信息接收時間戳。

其中，第一個個信息"A"指明了是一個Responese信息,是對poll信息的應答(ack)

 case 'P':
      /* Retrieve poll reception timestamp. */
      poll_rx_ts = get_rx_timestamp_u64();
      /* Set expected delay and timeout for final message reception. */
//                                  dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
//                                  dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
     msg_f_send.messageData[0]='A';//Poll ack message
     //后面修改這個數據長度
     dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
     dwt_writetxfctrl(psduLength, 0);
     dwt_starttx(DWT_START_TX_IMMEDIATE);
     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
     { };
     break;

關於發送response信息的地址，在收到信息的時候，會把源地址保存在目標地址，所以這個信息一定是返回給A節點的。

            dwt_readrxdata(rx_buffer, frame_len, 0);
            msg_f = (srd_msg_dsss*)rx_buffer;
            //copy source address as dest address
            msg_f_send.destAddr[0] = msg_f->sourceAddr[0];
            msg_f_send.destAddr[1] = msg_f->sourceAddr[1];
            //copy source seqNum
            msg_f_send.seqNum = msg_f->seqNum;

 

C 節點A收到節點B的response信息

節點A收到信息，會收集開始發送的poll信息時間戳以及接收到response的時間戳。

 poll_tx_ts = get_tx_timestamp_u64();
 resp_rx_ts = get_rx_timestamp_u64();

同時用delayed tx方式算出finnal信息發送時間，並一同打包發送給節點B

其中第一個信息“F”表明這個信息是final 信息

case 'A'://Poll ack message
    if(Tag_State == TAG_POLL_SENT)
    {
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
        //OLED_ShowString(0, 2,"Rec ACK");
        /* Retrieve poll transmission and response reception timestamp. */
        poll_tx_ts = get_tx_timestamp_u64();
        resp_rx_ts = get_rx_timestamp_u64();

        /* 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_tx_time =   dwt_readsystimestamphi32()  + 0x17cdc00/10;//1ms
        dwt_setdelayedtrxtime(final_tx_time);

        /* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
        final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8);
        //final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;

        msg_f_send.messageData[0]='F';//Final message
        /* Write all timestamps in the final message. See NOTE 10 below. */
        final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);
        final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
        final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
        dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
        dwt_writetxfctrl(psduLength, 0);
        dwt_starttx(DWT_START_TX_DELAYED);
        // dwt_starttx(DWT_START_TX_IMMEDIATE);
        while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
        { };
//  Tag_State = TAG_POLL_ACK;
        Tag_State = TAG_INIT;
    }
    break;

 

D 節點B收到節點A發來的信息后，讀取final信息的接收時間和上一個response信息的發送時間

    resp_tx_ts = get_tx_timestamp_u64();
    final_rx_ts = get_rx_timestamp_u64();

並通過函數final_msg_get_ts將節點A打包的三個時間信息解析出來

    final_msg_get_ts(&msg_f->messageData[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
    final_msg_get_ts(&msg_f->messageData[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
    final_msg_get_ts(&msg_f->messageData[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

后面就是利用這些時間戳計算距離信息了，完整代碼如下,算出距離信息，TWR算法就此結束。

case 'F':
    //  OLED_ShowString(0, 2,"Rec Final");
    /* Retrieve response transmission and final reception timestamps. */
    resp_tx_ts = get_tx_timestamp_u64();
    final_rx_ts = get_rx_timestamp_u64();

    /* Get timestamps embedded in the final message. */
    final_msg_get_ts(&msg_f->messageData[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
    final_msg_get_ts(&msg_f->messageData[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
    final_msg_get_ts(&msg_f->messageData[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;
    resp_tx_ts_32 = (uint32)resp_tx_ts;
    final_rx_ts_32 = (uint32)final_rx_ts;
    Ra = (double)(resp_rx_ts - poll_tx_ts);
    Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);
    Da = (double)(final_tx_ts - resp_rx_ts);
    Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);
    tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));

    tof = tof_dtu * DWT_TIME_UNITS;
    distance = tof * SPEED_OF_LIGHT;
    //distance = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);//距離減去矯正系數
    msg_f_send.messageData[0]='d';//Poll ack message
    int temp = (int)(distance*100);
    distance_count++;
    sum_distance =sum_distance+distance;
    {
        sprintf(dist_str, "an0:%3.2fm", distance);
        OLED_ShowString(0, 4,dist_str);
        distance_count= 0;
        sum_distance = 0;
    }

源碼下載請關注我們論壇51uwb.cn