51uwb.cn

 找回密码
 立即注册
查看: 11903|回复: 18
打印 上一主题 下一主题

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

[复制链接]

35

主题

1040

帖子

4469

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
4469
跳转到指定楼层
楼主
发表于 2020-3-19 08:52:01 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
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.     msg_f_send.seqNum = distance_seqnum;
  4.     msg_f_send.messageData[0]='P';//Poll message
  5.     //后面修改这个数据长度
  6.     dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
  7.     dwt_writetxfctrl(psduLength, 0);
  8.     dwt_starttx(DWT_START_TX_IMMEDIATE);
  9.     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
  10.     { };

  11.     dwt_enableframefilter(DWT_FF_DATA_EN);
  12.     dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*10);
  13.     dwt_rxenable(0);

  14.     /* Clear good RX frame event in the DW1000 status register. */
  15.     if(++distance_seqnum == 255)
  16.         distance_seqnum = 0;
  17.     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.         /* Compute final message transmission time. See NOTE 9 below. */
  10.         //    final_tx_time = ( resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;

  11.         final_tx_time =   dwt_readsystimestamphi32()  + 0x17cdc00/10;//1ms
  12.         dwt_setdelayedtrxtime(final_tx_time);

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

  16.         msg_f_send.messageData[0]='F';//Final message
  17.         /* Write all timestamps in the final message. See NOTE 10 below. */
  18.         final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts);
  19.         final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_RESP_RX_TS_IDX], resp_rx_ts);
  20.         final_msg_set_ts(&msg_f_send.messageData[FINAL_MSG_FINAL_TX_TS_IDX], final_tx_ts);
  21.         dwt_writetxdata(psduLength, (uint8 *)&msg_f_send, 0) ; // write the frame data
  22.         dwt_writetxfctrl(psduLength, 0);
  23.         dwt_starttx(DWT_START_TX_DELAYED);
  24.         // dwt_starttx(DWT_START_TX_IMMEDIATE);
  25.         while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
  26.         { };
  27. //  Tag_State = TAG_POLL_ACK;
  28.         Tag_State = TAG_INIT;
  29.     }
  30.     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.     /* Get timestamps embedded in the final message. */
  7.     final_msg_get_ts(&msg_f->messageData[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);
  8.     final_msg_get_ts(&msg_f->messageData[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);
  9.     final_msg_get_ts(&msg_f->messageData[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);

  10.     /* Compute time of flight. 32-bit subtractions give correct answers even if clock has wrapped. See NOTE 10 below. */
  11.     poll_rx_ts_32 = (uint32)poll_rx_ts;
  12.     resp_tx_ts_32 = (uint32)resp_tx_ts;
  13.     final_rx_ts_32 = (uint32)final_rx_ts;
  14.     Ra = (double)(resp_rx_ts - poll_tx_ts);
  15.     Rb = (double)(final_rx_ts_32 - resp_tx_ts_32);
  16.     Da = (double)(final_tx_ts - resp_rx_ts);
  17.     Db = (double)(resp_tx_ts_32 - poll_rx_ts_32);
  18.     tof_dtu = (int64)((Ra * Rb - Da * Db) / (Ra + Rb + Da + Db));

  19.     tof = tof_dtu * DWT_TIME_UNITS;
  20.     distance = tof * SPEED_OF_LIGHT;
  21.     //distance = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);//距离减去矫正系数
  22.     msg_f_send.messageData[0]='d';//Poll ack message
  23.     int temp = (int)(distance*100);
  24.     distance_count++;
  25.     sum_distance =sum_distance+distance;
  26.     {
  27.         sprintf(dist_str, "an0:%3.2fm", distance);
  28.         OLED_ShowString(0, 4,dist_str);
  29.         distance_count= 0;
  30.         sum_distance = 0;
  31.     }
复制代码

回复

使用道具 举报

0

主题

13

帖子

26

积分

新手上路

Rank: 1

积分
26
沙发
发表于 2020-4-26 14:09:10 | 只看该作者
讲解的比较透彻,谢谢!
回复

使用道具 举报

6

主题

43

帖子

47

积分

新手上路

Rank: 1

积分
47
板凳
发表于 2020-5-18 10:12:36 | 只看该作者
为什么 get_tx_timestamp_u64 这个要放在接收到正确帧之后,而不能直接放到发送完毕之后,就去获取时间戳呢?
回复

使用道具 举报

35

主题

1040

帖子

4469

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
4469
地板
 楼主| 发表于 2020-5-18 14:22:36 | 只看该作者
dw1000_2020 发表于 2020-5-18 10:12
为什么 get_tx_timestamp_u64 这个要放在接收到正确帧之后,而不能直接放到发送完毕之后,就去获取时间戳呢?

可以,效果是一样的
回复

使用道具 举报

2

主题

22

帖子

404

积分

中级会员

Rank: 3Rank: 3

积分
404
5#
发表于 2021-6-29 17:00:49 | 只看该作者
请问这种中断方式怎么调试呢?
标签发送了poll消息,基站接收到poll消息并发送了response消息,但是标签没有接收到response消息或者基站没有收到final消息该怎么调试?
回复

使用道具 举报

0

主题

2

帖子

8

积分

新手上路

Rank: 1

积分
8
6#
发表于 2021-8-10 16:17:46 | 只看该作者
这个能支持多标签,多基站网络情况下,测距吗
回复

使用道具 举报

0

主题

9

帖子

9

积分

新手上路

Rank: 1

积分
9
7#
发表于 2021-9-2 10:20:59 | 只看该作者
讲解的很透彻   感谢分享
回复

使用道具 举报

0

主题

3

帖子

3

积分

新手上路

Rank: 1

积分
3
8#
发表于 2021-10-15 17:00:40 | 只看该作者
讲解的很透彻,感谢分享
回复

使用道具 举报

1

主题

13

帖子

32

积分

游客

积分
32
9#
发表于 2021-11-16 11:53:26 | 只看该作者
分析很透彻,学到了!
回复

使用道具 举报

0

主题

1

帖子

4

积分

新手上路

Rank: 1

积分
4
10#
发表于 2021-11-19 19:04:46 | 只看该作者
请问能用在STM32F1上吗
回复

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

bphero Inc.  

GMT+8, 2024-11-1 08:11 , Processed in 0.019663 second(s), 5 queries , File On.

Powered by Discuz! X3.3

© 2001-2017 Comsenz Inc. Template By 【未来科技】【 www.wekei.cn 】

快速回复 返回顶部 返回列表