TWR ranging framework UWB DWM1000 open source project to achieve DW1000 user manual Chinese version of Appendix 3: two-way ranging (Two-Way Ranging)

TWR ranging method is a method of the current promotion of a ranging, a ranging method is currently used by most users, specific principle below

See specific algorithm: DW1000 user manual Chinese version of Appendix 3: two-way ranging (Two-Way Ranging)

The principle is very simple, node A sends information to the Poll Node B, Node B answers Responese, node A receives the reply message and then reply to a Final.

In the framework of our implementation code it is very easy 

A first message code Poll

Sending to the Node B, when we tested, Node A Node B address = address + 1

Transmitted data packet, the first information is all "P" indicates the message is a 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;

Poll Node B receives the message B, and returns information Responese

Poll information to the Node B when receiving timestamp information read by the poll get_rx_timestamp_u64 ().

Wherein the first information is all "A" indicates a Responese message is a response to the poll message (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;

For the address to send response information at the time of receipt of the information, we will target the source address stored in the address, so this information must be returned to the A node.

            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;

 

A Node C Node B receives response information

Node A receives the message, the time stamp information collecting poll transmission and reception of the response time stamp.

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

Simultaneously calculating finnal information send time delayed tx, packaged and sent to the Node B along with

Where the first message "F" indicates that this is the final message information

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;

 

After Node B receives the information D sent by the node A, the final reading information on a response reception time and transmission time information

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

Final_msg_get_ts function by three time information of the node A packaged parsed

    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);

Behind is the use of these timestamps to calculate distance information, and complete the code below calculates the distance information, TWR algorithm to an end.

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;
    }

Download Source Forum 51uwb.cn Follow us