Decawave官方双边测距(一对一)原理介绍及代码实现

Decawave官方双边测距(一对一)原理介绍及代码实现

一、运行平台
运行软件平台:Keil5
运行硬件平台:STM32
开发板型号:UWB-S1学习板
开发板淘宝链接：https://item.taobao.com/item.htm?spm=a1z10.5-c.w4002-23565193320.10.6e6c3f96tF7wds&id=572212584700

二、TOF/单边测距/双边测距概念介绍
Uwb常用测距方法有两种：飞行时间测量(TOF)和到达时间差(TDoA),我们这里将详细介绍飞行时间测量(TOF)的方法。

2.1 TOF
飞行时间测量法(Time of Flight,TOF)是一种双向测距技术,它通过测量两个设备间UWB信号的飞行时间来计算距离(注:所有的双向测距算法并不由DW1000来完成，DW1000仅负责记录下发送/接受数据包的时间戳)。根据UWB数据交互数量可分为：
①单边双向测距
②双边双向测距

2.2 单边双向测距
<单边测距Single-sided Two-WAY Ranging>具体流程：设备A首先向设备B发出一个数据包，并记录下发包时刻Ta1，设备B收到数据包后，记下收包时刻Tb1。之后设备B等待Treply时刻，在Tb2(Tb2 = Tb1 + Treply)时刻，向设备A发送一个数据包，设备A收到数据包后记下Ta2.然后可以算出电磁波在空中的飞行时间Tprop，飞行时间乘以光速即为两个设备间的距离。

因为设备A和设备B使用各自独立的时钟源，时钟都会有一定的偏差，假设设备A和设备B时钟的实际频率是预期频率的eA和eB倍，那么因为时钟偏差引入的误差error为：

设备A和B的时钟偏差都会对Tprop值造成影响，并且直接影响我们的测量精度，因为光速是30cm/ns，所以很小的时钟偏差也会对测量结果造成很大影响，而且这种影响是SS测距方式无法避免的。也因此SS测距很少被采用，大部分情况下我们都使用DS测距的方式。

2.3双边双向测距
<双边测距Double-sided Two-WAY Ranging>具体流程:DS测距在SS测距的基础上增加一次通讯，两次通讯的时间可以互相弥补(因为时间偏移引入的误差)。

使用DS测距方式时钟引入的误差为

假设设备A和设备B的时钟精度是20ppm（很差），1ppm为百万分之一，那么Ka和Kb分别是0.99998或者1.00002，ka和kb分别是设备A、B时钟的实际频率和预期频率的比值。设备A、B相距100m，电磁波的飞行时间是333ns。则因为时钟引入的误差为2033310-9秒，导致测距误差为2.2mm，可以忽略不计了。因此双边测距是最常采用的测距方式(Decawave官方也提供了双边测距的例程)。

三、官方测距代码实现
UWB-S1测距源码(官改)即使用了Decawave官方提供Double-sided two-way代码做基础,下面我们将对源码做详细介绍。

标签/基站模式切换方法如下
①基站模式:node_task(),注释tag_task()
②标签模式:tag_task(),注释node_task()

int main(void)
{
	init();	

//	tag_task();	//标签任务
	node_task();	//基站任务

	for(;;)
	{	
	}
}

3.1 基站/标签统一配置

初始化DW1000设备
dwt_initialise(DWT_LOADUCODE)

配置DW1000标签/基站通信信道的参数
dwt_configure(&config)

设置DW1000天线的延迟
dwt_setrxantennadelay(RX_ANT_DLY);
dwt_settxantennadelay(TX_ANT_DLY);

设置DW1000 LED指示灯与发送接受同步
dwt_setleds(3);

3.2 基站/标签流程
基站/标签流程图如下(图片版权并非作者)

3.2.1标签发送Poll

dwt_setrxaftertxdelay(POLL_TX_TO_RESP_RX_DLY_UUS);//设置标签发送Poll包后延迟打开接受时间
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);//设置接受超时时间
dwt_setpreambledetecttimeout(PRE_TIMEOUT);//设置前导码接受超时时间

dwt_writetxdata(sizeof(tx_poll_msg), tx_poll_msg, 0);//将Poll发送数据写入DW1000准备发送 
dwt_writetxfctrl(sizeof(tx_poll_msg), 0, 1);//设置发送Poll数据长度 

dwt_starttx(DWT_START_TX_IMMEDIATE | DWT_RESPONSE_EXPECTED);//立即打开发送

3.2.2 基站接受Poll

dwt_setpreambledetecttimeout(PRE_TIMEOUT);//设置前导码接受超时时间
dwt_setrxtimeout(0);//清除接受超时
dwt_rxenable(DWT_START_RX_IMMEDIATE);//立即开始接受

dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG);//清除DW1000接受成功寄存器

dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFL_MASK_1023;//读取DW1000接受数据长度
dwt_readrxdata(rx_buffer, frame_len, 0);//读取DW1000接受数据

3.2.3 基站发送Resp

poll_rx_ts = get_rx_timestamp_u64();//读取接受Poll时间戳值
resp_tx_time = (poll_rx_ts + (POLL_RX_TO_RESP_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算Resp数据包发送时间

dwt_setdelayedtrxtime(resp_tx_time);//设置Resp数据包发送时间
dwt_setrxaftertxdelay(RESP_TX_TO_FINAL_RX_DLY_UUS);//设置基站发送Resp包后延迟打开接受时间
dwt_setrxtimeout(FINAL_RX_TIMEOUT_UUS);//设置接受超时时间

dwt_writetxdata(sizeof(tx_resp_msg), tx_resp_msg, 0);//将Resp发送数据写入DW1000准备发送 
dwt_writetxfctrl(sizeof(tx_resp_msg), 0, 1); //设置发送Resp数据长度
dwt_starttx(DWT_START_TX_DELAYED | DWT_RESPONSE_EXPECTED);//设置延迟发送,

3.2.4 标签接受Resp

dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清除DW1000接受成功寄存器

dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;//读取DW1000接受数据长度
dwt_readrxdata(rx_buffer, frame_len, 0);//读取DW1000接受数据

3.2.5 标签发送Final

poll_tx_ts = get_tx_timestamp_u64();//读取发送Poll时间戳值
resp_rx_ts = get_rx_timestamp_u64();//读取接受Resp时间戳值

final_tx_time = (resp_rx_ts + (RESP_RX_TO_FINAL_TX_DLY_UUS * UUS_TO_DWT_TIME)) >> 8;//计算Final数据包发送时间
dwt_setdelayedtrxtime(final_tx_time);//设置Final数据包发送时间

dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);//将Final发送数据写入DW1000准备发送 
dwt_writetxfctrl(sizeof(tx_final_msg), 0, 1);//设置发送Final数据长度
ret = dwt_starttx(DWT_START_TX_DELAYED);//设置延迟发送,延迟发送时间为final_tx_time

3.2.6 基站接受Final

dwt_read32bitreg(SYS_STATUS_ID)) & (SYS_STATUS_RXFCG | SYS_STATUS_ALL_RX_TO | SYS_STATUS_ALL_RX_ERR)))
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_RXFCG | SYS_STATUS_TXFRS);//清除DW1000接受成功寄存器

dwt_read32bitreg(RX_FINFO_ID) & RX_FINFO_RXFLEN_MASK;//读取DW1000接受数据长度
dwt_readrxdata(rx_buffer, frame_len, 0);//读取DW1000接受数据

//开始3.2.7(基站-标签计算距离)

3.2.7 基站计算测距数据

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;

resp_tx_ts = get_tx_timestamp_u64();//读取发送Resp时间戳值
final_rx_ts = get_rx_timestamp_u64();//读取接受Final时间戳值

final_msg_get_ts(&rx_buffer[FINAL_MSG_POLL_TX_TS_IDX], &poll_tx_ts);//从标签发来数据中获取Poll发送时间戳
final_msg_get_ts(&rx_buffer[FINAL_MSG_RESP_RX_TS_IDX], &resp_rx_ts);//从标签发来数据中获取Resp接受时间戳
final_msg_get_ts(&rx_buffer[FINAL_MSG_FINAL_TX_TS_IDX], &final_tx_ts);//从标签发来数据中获取Final发送时间戳

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;//计算标签与基站距离值

memset(dist_str, 0, sizeof(dist_str));
sprintf(dist_str, "DIST: %3.2f m\n", distance);
port_tx_msg(dist_str,strlen(dist_str));//打印距离参数