说明
在由DW1000芯片的制造原厂提供的示例代码中，提供了SS-TWR方案的实现示例：
Example 6a: single-sided two-way ranging (SS TWR) initiator
Example 6b: single-sided two-way ranging responder
该示例以C代码形式，演示了一个简单的单边双向测距的实现过程，以下将对其原理和实际代码做分析
角色
单边双向的原理在之前的博客中已经说明过，不再赘述。在该示例中分为两个角色initiator和responder。
initiator是测距的主动发起者，通过主动发送第一帧数据和接收responder回复的第二帧数据，得到飞行时间从而算出两者之间的距离。
responder是测距的被动响应者，只会被动的接收数据并进行回复，不负责测距的数据计算。
时间点
在整个测距流程中，有关键的四个数据，分别是
poll_tx:initiator发送第一帧数据的时间点
poll_rx:responder接收第一帧数据的时间点
resp_tx:responder发送第二帧数据的时间点
resp_rx:initiator接收第二帧数据的时间点
只要得到这四个时间点，就可以算出飞行时间tof从而得到设备间的距离distance
\[distance = speed\_of\_light * tof\\
\]
\[tof = \frac{rtd\_init - rtd\_resp}{2}\\
\]
\[tof = \frac{(resp\_rx-poll\_tx)-(resp\_tx-poll\_rx)}{2}
\]
代码分析
0.initiator和responder初始化。对于responder在启动接收流程前只需要进行简单的通信参数配置和天线延迟配置即可
//responder.c
static dwt_config_t config = {
2, /* Channel number. */
DWT_PRF_64M, /* Pulse repetition frequency. */
DWT_PLEN_128, /* Preamble length. Used in TX only. */
DWT_PAC8, /* Preamble acquisition chunk size. Used in RX only. */
9, /* TX preamble code. Used in TX only. */
9, /* RX preamble code. Used in RX only. */
0, /* 0 to use standard SFD, 1 to use non-standard SFD. */
DWT_BR_6M8, /* Data rate. */
DWT_PHRMODE_STD, /* PHY header mode. */
(129 + 8 - 8) /* SFD timeout (preamble length + 1 + SFD length - PAC size). Used in RX only. */
};
/* Default antenna delay values for 64 MHz PRF. See NOTE 2 below. */
#define TX_ANT_DLY 16436
#define RX_ANT_DLY 16436
int main(void)
{
...............................
....................
/* Configure DW1000. See NOTE 5 below. */
dwt_configure(&config);
/* Apply default antenna delay value. See NOTE 2 below. */
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);
...............................
....................
while（1）
{
...............................
....................
}
}
但对于initiator，会多设置一步发送后自动延时一段时间后启动接收
//initiator.c
/* Set expected response's delay and timeout. See NOTE 1 and 5 below.
* As this example only handles one incoming frame with always the same delay and timeout, those values can be set here once for all. */
dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);
1.responder开启接收，随后轮询检测接收成功标志位，等待initiator发送第一帧数据
//responder.c
while(1)
{
/* Activate reception immediately. */
dwt_rxenable(DWT_START_RX_IMMEDIATE);
/* Poll for reception of a frame or error/timeout. See NOTE 6 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_ERR))){ };
...............................
....................
}
2.initiator发起通信，启动SS测距流程，填充数据帧到发送缓存，随后发送出去
//initiator.c
/* Write frame data to DW1000 and prepare transmission. See NOTE 7 below. */
tx_poll_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0); /* Zero offset in TX buffer. */
dwt_writetxfctrl(sizeof(tx_poll_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
/* 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_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);
发送完后轮询状态寄存器，等待responder回复第二帧数据，由于之前0阶段已经设置了发送后延时启动接收，所以不用在另外启动接收使能。
/* We assume that the transmission is achieved correctly, poll for reception of a frame or error/timeout. See NOTE 8 below. */
while (!((status_reg = dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
{ };
3.responder接收到第一帧数据后,把接收到的数据存到buff中，并确认是否为initiator发送的数据
while(1)
{
...............................
....................
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);
}
if (memcmp(rx_buffer, rx_poll_msg, ALL_MSG_COMMON_LEN) == 0)
{
...............................
....................
}
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
}
确认是initiator发送的第一帧数据后，取出接收到数据时的时间戳，并以此算出设置延迟发送的时间
uint32 resp_tx_time;
int ret;
/* Retrieve poll reception timestamp. */
poll_rx_ts = get_rx_timestamp_u64();
/* Compute final message transmission time. See NOTE 7 below. */
resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;
dwt_setdelayedtrxtime(resp_tx_time);
为了提高精度，把天线延迟也补偿进延迟时间
/* Response TX timestamp is the transmission time we programmed plus the antenna delay. */
resp_tx_ts = (((uint64)(resp_tx_time & 0xFFFFFFFEUL)) << 8) + TX_ANT_DLY;
最后得到responder接收到第一帧数据的时间poll_rx_ts，和发送第二帧的时间resp_tx_ts，把这两个参数写入到第二帧的数据段中准备发送给initiator
/* Write all timestamps in the final message. See NOTE 8 below. */
resp_msg_set_ts(&tx_resp_msg[RESP_MSG_POLL_RX_TS_IDX], poll_rx_ts);
resp_msg_set_ts(&tx_resp_msg[RESP_MSG_RESP_TX_TS_IDX], resp_tx_ts);
一切就绪后，启动延时发送，芯片到达设定好的时间点就会自动发送数据帧
/* Write and send the response message. See NOTE 9 below. */
tx_resp_msg[ALL_MSG_SN_IDX] = frame_seq_nb;
dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0); /* Zero offset in TX buffer. */
dwt_writetxfctrl(sizeof(tx_resp_msg), 0, 1); /* Zero offset in TX buffer, ranging. */
ret = dwt_starttx(DWT_START_TX_DELAYED);
...............................
....................
4.initiator轮询直到接收到responder回发数据，和responder一样，接收到数据后做基本的检查
/* 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 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_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 "SS 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)
{
。。。。。。
}
}
else
{
/* Clear RX error/timeout events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR);
/* Reset RX to properly reinitialise LDE operation. */
dwt_rxreset();
}
检测确认是responder回发的第二帧数据后，开始准备测距算法需要的时间戳数据，initiator先读出自己发第一帧数据的时间poll_tx_ts和收到第二帧数据的时间resp_rx_ts
uint32 poll_tx_ts, resp_rx_ts, poll_rx_ts, resp_tx_ts;
int32 rtd_init, rtd_resp;
float clockOffsetRatio ;
/* Retrieve poll transmission and response reception timestamps. See NOTE 9 below. */
poll_tx_ts = dwt_readtxtimestamplo32();
resp_rx_ts = dwt_readrxtimestamplo32();
随后从responder发来的数据帧中取出其收到第一帧数据包的时间poll_rx_ts和发送第二帧数据包的时间resp_tx_ts
/* Get timestamps embedded in response message. */
resp_msg_get_ts(&rx_buffer[RESP_MSG_POLL_RX_TS_IDX], &poll_rx_ts);
resp_msg_get_ts(&rx_buffer[RESP_MSG_RESP_TX_TS_IDX], &resp_tx_ts);
至此得到所有数据，代入公式即可计算出飞行时间和设备距离
/* Compute time of flight and distance, using clock offset ratio to correct for differing local and remote clock rates */
rtd_init = resp_rx_ts - poll_tx_ts;
rtd_resp = resp_tx_ts - poll_rx_ts;
tof = ((rtd_init - rtd_resp ) / 2.0) * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;
为了提高精度，还可以将initiator的晶振误差补偿进去，最后得到的公式就变为
/* Read carrier integrator value and calculate clock offset ratio. See NOTE 11 below. */
clockOffsetRatio = dwt_readcarrierintegrator() * (FREQ_OFFSET_MULTIPLIER * HERTZ_TO_PPM_MULTIPLIER_CHAN_2 / 1.0e6) ;
tof = ((rtd_init - rtd_resp * (1 - clockOffsetRatio)) / 2.0) * DWT_TIME_UNITS;
distance = tof * SPEED_OF_LIGHT;