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信息 - 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;
- }
复制代码
|