熊大UWB系列教程三：DW1000测距原理以及软件实现
有超宽带定位项目需求请加微信沟通 微信号： cc12131789
超宽带交流qq群：651967580
一、原理介绍
DW1000的测距原理在dw1000_user_manual文档附录三中有介绍，DW1000有两种测距方式，一种是SS测距（Single-sided Two-way Ranging），另外一种是DS测距（double-sided Two-way Ranging）。
1.Single-sided Two-way Ranging (SS)
具体流程是，设备A首先向设备B发出一个数据包，并记录下发包时刻Ta1，设备B收到数据包后，记下收包时刻Tb1。之后设备B等待Treply时刻，在Tb2（Tb2=Tb1+Treply）时刻，向设备A发送一个数据包，设备A收到数据包后记下时刻值Ta2。然后可以算出电磁波在空中的飞行时间Tprop，飞行时间乘以光速即为两个设备间的距离。
Tround= Ta2-Ta1
Treply=Tb2-Tb1
因为设备A和设备B使用各自独立的时钟源，时钟都会有一定的偏差，假设设备A和设备B时钟的实际频率是预期频率的eA和eB倍，那么因为时钟偏差引入的误差error为：
设备A和B的时钟偏差都会对Tprop值造成影响，并且直接影响我们的测量精度，因为光速是30cm/ns，所以很小的时钟偏差也会对测量结果造成很大影响，而且这种影响是SS测距方式无法避免的。也因此SS测距很少被采用，大部分情况下我们都使用下一种，DS测距的方式。
2.Double-sidedTwo-way Ranging（DS）
DS测距是在SS测距的基础上再增加一次通讯，两次通讯的时间可以互相弥补因为时钟偏移引入的误差。
使用DS测距方式时钟引入的误差为
假设设备A和设备B的时钟精度是20ppm（很差），1ppm为百万分之一，那么Ka和Kb分别是0.99998或者1.00002，ka和kb分别是设备A、B时钟的实际频率和预期频率的比值。设备A、B相距100m，电磁波的飞行时间是333ns。则因为时钟引入的误差为20*333*10-9秒，导致测距误差为2.2mm，可以忽略不计了。因此双边测距是最常采用的测距方式。下面我将介绍双边测距的代码实现。
二、Double-sidedTwo-way Ranging代码实现
关于dw1000芯片控制API在请看《DW1000_Software_API_Guide》官方手册。下面着重介绍测距的代码实现，具体的芯片初始化什么的，这里先略过，下节再讲。
如上图所示，完成一次DS测距需要6个步骤。
1.设备A发送POLL包。并记下发送时间T1。并在一段时间后打开RX。
2.设备B要提前打开接收，然后收到POLL包，记录时间T2.
3.设备B在T3（T3=T2+Treply1）时刻发送Response包，发送完之后打开RX.
4.设备A收到Response包，记录时刻T4。
5.设备A在T5(T5=T4+Treply2)发送Final包。
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。
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//写入发送数据
dwt_writetxfctrl(sizeof(tx_poll_msg), 0); //发送数据长度
/* 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. */ //发送数据长度
/* 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);//设定发送后打开接收的时间
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); //设定接收超时的时间
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//立即发送，并且在一定时间后打开接收。 dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);//设定发送后打开接收的时间
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS); //设定接收超时的时间
dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//立即发送，并且在一定时间后打开接收。2.设备B要提前打开接收，然后收到POLL包，记录时间T2.
3.设备B在T3（T3=T2+Treply1）时刻发送Response包，发送完之后打开RX.
dwt_setrxtimeout(0);//设定接收超时，0为没有超时
/* Activate reception immediately. */
dwt_rxenable(0);//打开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)))//不断查询寄存器直到接收成功
{ };
if (status_reg & SYS_STATUS_RXFCG)//接收成功
{
/* 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);//读取收到数据
}
/* 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) //比较是否正确
{
uint32 resp_tx_time;
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();//获得收到数据的时间T2 resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算response发送时间T3
dwt_setdelayedtrxtime(resp_tx_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);//设定延迟发送response包，之后打开接收4.设备A收到Response包，记录时刻T4。
5.设备A在T5(T5=T4+Treply2)发送Final包
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))) //设备A不断查询寄存器，直到成功接收
{ };
/* Increment frame sequence number after transmission of the poll message (modulo 256). */
frame_seq_nb++;
if (status_reg & SYS_STATUS_RXFCG) //成功接收
{
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. */ //将说有的时刻数据写入发送包，用于设备B计算
final_msg_set_ts(&tx_final_msg[FINAL_MSG_POLL_TX_TS_IDX], poll_tx_ts); //写入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);//延迟发送，不再接收
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));//计算公式
tof = tof_dtu * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;//距离=时间*光速
如果本教程对你有帮助，请留言支持，你的支持是熊大不断更新的动力。