UWB DWM1000 开源项目框架 之 TWR测距实现

蓝点无限

TWR测距方法是目前推广的一种测距方法，也是目前大多数用户使用的测距方法，具体原理如下图

原理很简单，节点A发送信息Poll给节点B，节点B应答Responese，节点A收到应答再回复一条Final信息。

具体算法参见:http://bphero.com.cn/forum.php?mod=viewthread&tid=70&fromuid=2

在我们框架里实现代码也是非常容易

A 第一个信息Poll的代码

发送给节点B，我们测试的时候，节点B地址=节点A地址+1

发送的数据包，第一个个信息"P"指明了是一个Poll信息

1. msg_f_send.destAddr[0] =(SHORT_ADDR+1) &0xFF;
   
2.     msg_f_send.destAddr[1] =  ((SHORT_ADDR+1)>>8) &0xFF;
   
3. 
   
4.     msg_f_send.seqNum = distance_seqnum;
   
5.     msg_f_send.messageData[0]='P';//Poll message
   
6.     //后面修改这个数据长度
   
7.     dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
   
8.     dwt_writetxfctrl(psduLength, 0);
   
9.     dwt_starttx(DWT_START_TX_IMMEDIATE);
   
10.     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
    
11.     { };
    
12. 
    
13.     dwt_enableframefilter(DWT_FF_DATA_EN);
    
14.     dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*10);
    
15.     dwt_rxenable(0);
    
16. 
    
17.     /* Clear good RX frame event in the DW1000 status register. */
    
18.     if(++distance_seqnum == 255)
    
19.         distance_seqnum = 0;
    
20.     Tag_State = TAG_POLL_SENT;

复制代码

B 节点B收到Poll信息，并返回Responese 信息

节点B接到Poll信息的时候，通过get_rx_timestamp_u64()读取了poll信息接收时间戳。

其中，第一个个信息"A"指明了是一个Responese信息,是对poll信息的应答(ack)

1. case 'P':
   
2.       /* Retrieve poll reception timestamp. */
   
3.       poll_rx_ts = get_rx_timestamp_u64();
   
4.       /* Set expected delay and timeout for final message reception. */
   
5. //                                  dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);
   
6. //                                  dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);
   
7.      msg_f_send.messageData[0]='A';//Poll ack message
   
8.      //后面修改这个数据长度
   
9.      dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
   
10.      dwt_writetxfctrl(psduLength, 0);
    
11.      dwt_starttx(DWT_START_TX_IMMEDIATE);
    
12.      while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
    
13.      { };
    
14.      break;

复制代码

关于发送response信息的地址，在收到信息的时候，会把源地址保存在目标地址，所以这个信息一定是返回给A节点的。

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

复制代码

C 节点A收到节点B的response信息

节点A收到信息，会收集开始发送的poll信息时间戳以及接收到response的时间戳。

1. poll_tx_ts = get_tx_timestamp_u64();
   
2. resp_rx_ts = get_rx_timestamp_u64();

复制代码

同时用delayed tx方式算出finnal信息发送时间，并一同打包发送给节点B

其中第一个信息“F”表明这个信息是final 信息

1. case 'A'://Poll ack message
   
2.     if(Tag_State == TAG_POLL_SENT)
   
3.     {
   
4.         dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);
   
5.         //OLED_ShowString(0, 2,"Rec ACK");
   
6.         /* Retrieve poll transmission and response reception timestamp. */
   
7.         poll_tx_ts = get_tx_timestamp_u64();
   
8.         resp_rx_ts = get_rx_timestamp_u64();
   
9. 
   
10.         /* Compute final message transmission time. See NOTE 9 below. */
    
11.         //    final_tx_time = ( resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
    
12. 
    
13.         final_tx_time =   dwt_readsystimestamphi32()  + 0x17cdc00/10;//1ms
    
14.         dwt_setdelayedtrxtime(final_tx_time);
    
15. 
    
16.         /* Final TX timestamp is the transmission time we programmed plus the TX antenna delay. */
    
17.         final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8);
    
18.         //final_tx_ts = (((uint64)(final_tx_time & 0xFFFFFFFE)) << 8) + TX_ANT_DLY;
    
19. 
    
20.         msg_f_send.messageData[0]='F';//Final message
    
21.         /* Write all timestamps in the final message. See NOTE 10 below. */
    
22.         final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);
    
23.         final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
    
24.         final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
    
25.         dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
    
26.         dwt_writetxfctrl(psduLength, 0);
    
27.         dwt_starttx(DWT_START_TX_DELAYED);
    
28.         // dwt_starttx(DWT_START_TX_IMMEDIATE);
    
29.         while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
    
30.         { };
    
31. //  Tag_State = TAG_POLL_ACK;
    
32.         Tag_State = TAG_INIT;
    
33.     }
    
34.     break;

复制代码

D 节点B收到节点A发来的信息后，读取final信息的接收时间和上一个response信息的发送时间

1. resp_tx_ts = get_tx_timestamp_u64();
   
2.     final_rx_ts = get_rx_timestamp_u64();

复制代码

并通过函数final_msg_get_ts将节点A打包的三个时间信息解析出来

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

复制代码

后面就是利用这些时间戳计算距离信息了，完整代码如下,算出距离信息，TWR算法就此结束。

1. case 'F':
   
2.     //  OLED_ShowString(0, 2,"Rec Final");
   
3.     /* Retrieve response transmission and final reception timestamps. */
   
4.     resp_tx_ts = get_tx_timestamp_u64();
   
5.     final_rx_ts = get_rx_timestamp_u64();
   
6. 
   
7.     /* Get timestamps embedded in the final message. */
   
8.     final_msg_get_ts(&msg_f->messageData[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
   
9.     final_msg_get_ts(&msg_f->messageData[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
   
10.     final_msg_get_ts(&msg_f->messageData[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);
    
11. 
    
12.     /* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
    
13.     poll_rx_ts_32 = (uint32)poll_rx_ts;
    
14.     resp_tx_ts_32 = (uint32)resp_tx_ts;
    
15.     final_rx_ts_32 = (uint32)final_rx_ts;
    
16.     Ra = (double)(resp_rx_ts - poll_tx_ts);
    
17.     Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);
    
18.     Da = (double)(final_tx_ts - resp_rx_ts);
    
19.     Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);
    
20.     tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));
    
21. 
    
22.     tof = tof_dtu * DWT_TIME_UNITS;
    
23.     distance = tof * SPEED_OF_LIGHT;
    
24.     //distance = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);//距离减去矫正系数
    
25.     msg_f_send.messageData[0]='d';//Poll ack message
    
26.     int temp = (int)(distance*100);
    
27.     distance_count++;
    
28.     sum_distance =sum_distance+distance;
    
29.     {
    
30.         sprintf(dist_str, "an0:%3.2fm", distance);
    
31.         OLED_ShowString(0, 4,dist_str);
    
32.         distance_count= 0;
    
33.         sum_distance = 0;
    
34.     }

复制代码

源码下载:https://bitbucket.org/tuzhuke/51base_twr_f1

huaohui

讲解的比较透彻，谢谢！

dw1000_2020

为什么 get_tx_timestamp_u64 这个要放在接收到正确帧之后,而不能直接放到发送完毕之后,就去获取时间戳呢?

蓝点无限

dw1000_2020 发表于 2020-5-18 10:12
为什么 get_tx_timestamp_u64 这个要放在接收到正确帧之后,而不能直接放到发送完毕之后,就去获取时间戳呢?

可以，效果是一样的

dragon_L

请问这种中断方式怎么调试呢？
标签发送了poll消息，基站接收到poll消息并发送了response消息，但是标签没有接收到response消息或者基站没有收到final消息该怎么调试？

程序猿

这个能支持多标签，多基站网络情况下，测距吗

wangxianglong!!

讲解的很透彻   感谢分享

zhouyougm

讲解的很透彻，感谢分享

adviil

分析很透彻，学到了！

15847690328

请问能用在STM32F1上吗