1. ホーム

Xiong Da UWB チュートリアルシリーズ III: DW1000 レンジング原理とソフトウェア実装

2022-02-18 10:02:45

超広帯域測位プロジェクトのニーズがあれば、WeChat通信微信を追加してください:cc12131789

超ブロードバンド通信qq群。651967580

I. 原理の紹介

DW1000のレンジング原理は、dw1000_user_manual文書のAppendix IIIに記載されている。DW1000には、SSレンジング(Single-sided Two-way Ranging)とDSレンジング(Double-sided Two-way Ranging)という2つのレンジング方式があります。

1. 片側双方向通信(SS)

具体的な処理としては、まず機器Aが機器Bにパケットを送信して送信の瞬間Ta1を記録し、パケットを受信した機器Bは受信の瞬間Tb1を記録する。その後、機器BはTreplyの瞬間を待ち、Tb2(Tb2=Tb1+Treply)の瞬間に機器Aへパケットを送信し、パケット受信後、機器Aはその瞬間値Ta2を記録する。そして、電磁波の空中飛行時間Tpropを計算し、その飛行時間に光速を掛けたものが2つの装置間の距離となります。

Tround= Ta2-Ta1

トレープリー=Tb2-Tb1

機器Aと機器Bはそれぞれ独立したクロックソースを使用しているため、両者のクロックには多少のずれが生じます。デバイスAとデバイスBのクロックの実際の周波数が、期待される周波数のeA倍とeB倍であると仮定すると、クロックの偏差エラーによって生じる誤差は

デバイスAとBの両方のクロックのずれがTprop値に影響し、測定精度に直結します。光速は30cm/nsなので、わずかなクロックのずれが結果に大きく影響し、この影響はSSレンジングでは避けることができません。そのため、SSレンジングを使うことはほとんどなく、ほとんどの場合、次の方法であるDSレンジングを使用します。

2. ダブルサイド2ウェイレンジ(DS)

DSレンジングとは、SSレンジングの上に追加される通信であり、2つの通信のタイミングは、クロックオフセットによって生じる誤差を互いに補償することができます。

DSレンジング方式のクロックを使用した場合に発生する誤差は

デバイスAとデバイスBのクロックの精度を20ppm(非常に悪い)、1ppmを100万分の1とすると、KaとKbはそれぞれ0.99998または1.00002です。kaとkbはそれぞれデバイスAとBのクロックの実際の周波数と予想周波数の比です。装置A、B間の距離は100m、電磁波の飛行時間は333nsである。時計によってもたらされる誤差は20*333*10-9秒となり、レンジ誤差は2.2mmとなり、無視できるレベルである。したがって、バイラテラルレンジングが最もよく使われる測距方法である。以下、バイラテラルレンジングのコード実装を説明します。

II. Double-sidedTwo-way Rangingコードの実装

dw1000のチップ制御APIについては、DW1000_Software_API_Guideの公式マニュアルを参照してください。以下は、レンジングコードの実装に焦点を当て、具体的なチップの初期化などは、とりあえずここでは省略し、次節で説明します。

上記のように、DSレンジングセッションを完了するには6つのステップが必要です。

1. 機器AはPOLLパケットを送信する。そして送信時間T1.をメモし、しばらくしてからRXをONにする。

2. 機器Bはあらかじめ受信を開始してからPOLLパケットを受信し、時間T2を記録しておく。

3. 機器BはT3(T3=T2+Treply1)の瞬間に応答パケットを送信し、送信後にRXを開く。

4. デバイスAはレスポンスパケットを受信し、時刻T4を記録する。

5. デバイスAはT5でFinalパケットを送信する(T5=T4+Treply2)。

6. デバイスBはFinalパッケージを受信し、時間T6を記録する。

計算します。

式の導出。

Tround1 = Treply1 + 2Tprop;

Tround2 = Treply2 + 2Tprop;

Tround1*Tround2 - Treply1*Treply2 = 4Tprop² + 2Tprop*Treply1 + 2Tprop*Treply2。

Tround1 +Tround2 + Treply1 + Treply2 = 4Tprop + 2Treply1 + 2Treply2;

つまり、Tropは上の式と等しいのです。

ここで、具体的な時間の計算をします。

Tround1 = T4 - T1

Tround2 = T6 - T3

Treply1 = T3 - T2

Treply2 = T5 - T4

Tpropは電磁波の飛行時間であり、測定距離に対して光速を乗じたものである。

各ステップの具体的なコードは、上記6つのステップごとに以下に説明します。

1. デバイスAはPOLLパケットを送信する。そして、送信時間T1を書き留め、しばらくしてからRXをONにする。

        dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//write send data
        dwt_writetxfctrl(sizeof(tx_poll_msg), 0); //Send data length

        /* 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. */

	     // Send data length

        /* 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_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);// set the time to open the receive after sending
    	dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); //Set the time to receive timeout
        dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);// send immediately and open receive after a certain time.

	dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);//Set the time to send and open receive after a certain time
    	dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); //Set the time to receive timeout
        dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);// send immediately and open receive after a certain time.

 dwt_setrxtimeout(0);//Set receive timeout, 0 is no timeout

        /* Activate reception immediately. */
        dwt_rxenable(0);// turn on RX

        /* 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)))// Keep querying registers until reception is successful
        { };

        if (status_reg & SYS_STATUS_RXFCG)//receive success
        {
           

            /* Clear good RX frame event in the DW1000 status register. */
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);

            /* A frame has been received, read it into the local buffer. */
            frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
            if (frame_len <= RX_BUFFER_LEN)
            {
                dwt_readrxdata(rx_buffer, frame_len, 0);//read received data
            }

            /* 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 = rx_buffer[5];
							rx_poll_msg[5] = TAG_ID;
							tx_resp_msg[5] = TAG_ID;
							rx_final_msg[5] = TAG_ID;
            if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) //compare for correctness
            {
                uint32 resp_tx_time;

                /* Retrieve poll reception timestamp. */
                poll_rx_ts = get_rx_timestamp_u64();// Get the time T2 of the received data

 resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//calculate response send time T3
                dwt_setdelayedtrxtime(resp_tx_time);//set send time T3

                /* Set expected delay and timeout for final message reception. */
                dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
                dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);

                /* Write and send the response message. see NOTE 9 below.*/
						// memcpy(&tx_resp_msg[11],&dis,4);
                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);//set a delay to send the response packet and open the receive afterwards

 while (! ((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) //Device A keeps querying registers until successful reception
        { };

        /* Increment frame sequence number after transmission of the poll message (modulo 256). */
        frame_seq_nb++;

        if (status_reg & SYS_STATUS_RXFCG) // Successfully received
        {
            uint32 frame_len;

            /* Clear good RX frame event and TX frame sent in the DW1000 status register. */
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

            /* A frame has been received, read it into the local buffer. */
            frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
            if (frame_len <= RX_BUF_LEN)
            {
                dwt_readrxdata(rx_buffer, frame_len, 0);
            }

            /* 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 (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0)
            {
                uint32 final_tx_time;

								memcpy(&rec_dist,&rx_buffer[11],4);
                /* 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;
                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) + TX_ANT_DLY;

                /* Write all timestamps in the final message. See NOTE 10 below. */ // Write the data of said timestamps to the send packet for device B calculation
                final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts); //write to T1
                final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts); //T4
                final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts); //T5

                /* 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);
                dwt_writetxfctrl(sizeof(tx_final_msg), 0);
                dwt_starttx(DWT_START_TX_DELAYED);//delay sending, no more receiving

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

                if (status_reg & SYS_STATUS_RXFCG)
                {
                    /* Clear good RX frame event and TX frame sent in the DW1000 status register. */
                    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

                    /* A frame has been received, read it into the local buffer. */
                    frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
                    if (frame_len <= RX_BUF_LEN2)
                    {
                        dwt_readrxdata(rx_buffer, frame_len, 0);
                    }

                    /* 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 (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN) == 0)
                    {
                        uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
                        uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
                        double Ra, Rb, Da, Db;
                        int64 tof_dtu;

                        /* 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(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
                        final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
                        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;
                        resp_tx_ts_32 = (uint32)resp_tx_ts;
                        final_rx_ts_32 = (uint32)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));//Calculate the formula

                       

2. デバイスBは早期に受信を開始し、その後POLLパケットを受信し、時間T2を記録する必要があります。

3. 機器Bは時刻T3に応答パケットを送信し(T3=T2+Treply1)、送信後にRXをオープンする。

 dwt_setrxtimeout(0);//Set receive timeout, 0 is no timeout

        /* Activate reception immediately. */
        dwt_rxenable(0);// turn on RX

        /* 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)))// Keep querying registers until reception is successful
        { };

        if (status_reg & SYS_STATUS_RXFCG)//receive success
        {
           

            /* Clear good RX frame event in the DW1000 status register. */
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);

            /* A frame has been received, read it into the local buffer. */
            frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;
            if (frame_len <= RX_BUFFER_LEN)
            {
                dwt_readrxdata(rx_buffer, frame_len, 0);//read received data
            }

            /* 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 = rx_buffer[5];
							rx_poll_msg[5] = TAG_ID;
							tx_resp_msg[5] = TAG_ID;
							rx_final_msg[5] = TAG_ID;
            if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0) //compare for correctness
            {
                uint32 resp_tx_time;

                /* Retrieve poll reception timestamp. */
                poll_rx_ts = get_rx_timestamp_u64();// Get the time T2 of the received data


 resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//calculate response send time T3
                dwt_setdelayedtrxtime(resp_tx_time);//set send time T3

                /* Set expected delay and timeout for final message reception. */
                dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
                dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);

                /* Write and send the response message. see NOTE 9 below.*/
						// memcpy(&tx_resp_msg[11],&dis,4);
                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);//set a delay to send the response packet and open the receive afterwards

4. デバイスAはレスポンスパケットを受信し、その瞬間T4を記録する。

5. デバイスAはT5でファイナルパケットを送信する(T5=T4+Treply2)。

 while (! ((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) //Device A keeps querying registers until successful reception
        { };

        /* Increment frame sequence number after transmission of the poll message (modulo 256). */
        frame_seq_nb++;

        if (status_reg & SYS_STATUS_RXFCG) // Successfully received
        {
            uint32 frame_len;

            /* Clear good RX frame event and TX frame sent in the DW1000 status register. */
            dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

            /* A frame has been received, read it into the local buffer. */
            frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
            if (frame_len <= RX_BUF_LEN)
            {
                dwt_readrxdata(rx_buffer, frame_len, 0);
            }

            /* 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 (memcmp(rx_buffer, rx_resp_msg, ALL_MSG_COMMON_LEN) == 0)
            {
                uint32 final_tx_time;

								memcpy(&rec_dist,&rx_buffer[11],4);
                /* 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;
                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) + TX_ANT_DLY;

                /* Write all timestamps in the final message. See NOTE 10 below. */ // Write the data of said timestamps to the send packet for device B calculation
                final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts); //write to T1
                final_msg_set_ts(&tx_final_msg[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts); //T4
                final_msg_set_ts(&tx_final_msg[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts); //T5

                /* 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);
                dwt_writetxfctrl(sizeof(tx_final_msg), 0);
                dwt_starttx(DWT_START_TX_DELAYED);//delay sending, no more receiving

6. 機器BはFinalパケットを受信し、時刻T6を記録する。

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

                if (status_reg & SYS_STATUS_RXFCG)
                {
                    /* Clear good RX frame event and TX frame sent in the DW1000 status register. */
                    dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);

                    /* A frame has been received, read it into the local buffer. */
                    frame_len = dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;
                    if (frame_len <= RX_BUF_LEN2)
                    {
                        dwt_readrxdata(rx_buffer, frame_len, 0);
                    }

                    /* 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 (memcmp(rx_buffer, rx_final_msg, ALL_MSG_COMMON_LEN) == 0)
                    {
                        uint32 poll_tx_ts, resp_rx_ts, final_tx_ts;
                        uint32 poll_rx_ts_32, resp_tx_ts_32, final_rx_ts_32;
                        double Ra, Rb, Da, Db;
                        int64 tof_dtu;

                        /* 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(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
                        final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
                        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;
                        resp_tx_ts_32 = (uint32)resp_tx_ts;
                        final_rx_ts_32 = (uint32)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));//Calculate the formula

                       

このチュートリアルが役に立った場合は、コメントを残して応援してください。あなたのサポートがあるからこそ、ベアーズは更新し続けることができるのです。