【UWB定位】 - DWM1000模块调试简单心得 - 3
UWB定位 - DWM1000模块调试简单心得 - 1
UWB定位 - DWM1000模块调试简单心得 - 2
前俩篇介绍了简单的一基站一标签TOF方式测距，第三篇我们来搭建一个 一标签三基站 的定位demo。
目的 : 标签与三个基站分别测距，基站得到数据后统一汇总到一个总基站，总基站通过串口将同一时刻三基站与标签的距离值输出到串口调试助手，或者想实现定位的话，我们可以直接写到管道、文件里，由电脑linux端处理数据，进而结合三点定位算法，实现标签简单定位，今天我们主要完成1对3测距输出。
环境 : 4个stm32+DWM1000模块（3基站1标签），keli软件、标签供电电池、usb 转 TTL、sscom串口调试助手。
正文：
1、其实三基站一标签就是在 1对 1的情况多了俩路数据而已，然后就是将多的俩路数据汇总到一个总基站上面总基站。 我们知道TOF测距方式其实就是标签与基站的数据包的发送与回应。数据包是什么，就是带有帧头、帧尾、目的、源的一帧数据。即代码里（ W A V E 即为目的地址和源地址，这是标签的第一次请求数据包，对应的我们看基站的即是 V E W A ，这个我们可以自行修改，基站与标签对应就行）
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 tx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8 rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8 tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};具体的数据包解析代码最后有注释，我在这粘一下吧，大家自行翻译对着位数看一下哈，很简单
* The first 10 bytes of those frame are common and are composed of the following fields:
* - byte 0/1: frame control (0x8841 to indicate a data frame using 16-bit addressing).
* - byte 2: sequence number, incremented for each new frame.
* - byte 3/4: PAN ID (0xDECA).
* - byte 5/6: destination address, see NOTE 3 below.
* - byte 7/8: source address, see NOTE 3 below.
* - byte 9: function code (specific values to indicate which message it is in the ranging process).
2、经过上面我们了解，我们先把一标签对三基站的测距做好之后在做基站数据的汇总不就OK了吗。上边说过，基站与标签是通过数据包的收发来进行的，1基站1标签是一路数据的收发，那我们要实现1标签3基站就多加俩个数据包，然后标签轮询发送（不要阻塞）或者以多任务的形式来发送接收不就OK了。数据包的主要区别就在于目的和源地址的区别。我们可以自己从新定义2类数据包或是用指针数组都可以，咱就用看起来麻烦点的重新分别定义来写吧，如下标签、基站部分参考。
标签：
//标签部分数据包定义
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 tx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8 rx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8 tx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static uint8 tx_poll_msg_station2[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'T', 'I', 'O', 'N', 0x21, 0, 0};
static uint8 rx_resp_msg_station2[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'N', 'O', 'T', 'I', 0x10, 0x02, 0, 0, 0, 0};
static uint8 tx_final_msg_station2[] = {0x41, 0x88, 0, 0xCA, 0xDE,'T', 'I', 'O', 'N', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static uint8 tx_poll_msg_station3[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'F', 'I', 'G', 'H', 0x21, 0, 0};
static uint8 rx_resp_msg_station3[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'H', 'G', 'I', 'F', 0x10, 0x02, 0, 0, 0, 0};
static uint8 tx_final_msg_station3[] = {0x41, 0x88, 0, 0xCA, 0xDE,'F', 'I', 'G', 'H', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};总基站1：（其中 rx_station2_msg[]与 rx_station3_msg[]为接收分基站1号和2号的数据帧包格式，即数据汇总）
//总基站1号 rx_station2_msg[]与 rx_station3_msg[]为接收分基站1/2的数据帧包
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 rx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x21, 0, 0};
static uint8 tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'V', 'E', 'W', 'A', 0x10, 0x02, 0, 0, 0, 0};
static uint8 rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'W', 'A', 'V', 'E', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static uint8 rx_station2_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'S', 'I', 'G', 'N', 0x23, 0, 0, 0, 0,0,0,0,0,0,0};
static uint8 rx_station3_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'T', 'H', 'I', 'R', 0x23, 0, 0, 0, 0,0,0,0,0,0,0};
/* Length of the common part of the message (up to and including the function code, see NOTE 2 below). */分基站2（数据包与总基站和标签都是对应的）：
//分基站1
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 rx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'T', 'I', 'O', 'N', 0x21, 0, 0};
static uint8 tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'N', 'O', 'T', 'I', 0x10, 0x02, 0, 0, 0, 0};
static uint8 rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE,'T', 'I', 'O', 'N', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static uint8 tx_final_msg_station2[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'S', 'I', 'G', 'N', 0x23, 0, 0, 0, 0,0,0,0,0,0,0};//发送给总基站的数据包分基站 3：
/* Frames used in the ranging process. See NOTE 2 below. */
static uint8 rx_poll_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'F', 'I', 'G', 'H', 0x21, 0, 0};
static uint8 tx_resp_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'H', 'G', 'I', 'F', 0x10, 0x02, 0, 0, 0, 0};
static uint8 rx_final_msg[] = {0x41, 0x88, 0, 0xCA, 0xDE,'F', 'I', 'G', 'H', 0x23, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0};
static uint8 tx_final_msg_station3[] = {0x41, 0x88, 0, 0xCA, 0xDE, 'T', 'H', 'I', 'R', 0x23, 0, 0, 0, 0,0,0,0,0,0,0};然后就是在标签中的代码里多定义俩个帧序列号（标签对应的3个基站 所以需要3个）（分基站其实只需要定义俩个就可以了，一个是与标签的，一个是与总基站的）：
//总基站1
/* Frame sequence number, incremented after each transmission. */
static uint8 frame_seq_nb = 0;
static uint8 frame_seq_nb_station2 = 0;
static uint8 frame_seq_nb_station3 = 0; 剩下的其实就是标签的轮询发送了，其实就是在之前1对1基础上copy俩次完整的发送接收步骤，代码就不贴了太多了自己懒没有精简，具体结构框架为（其中需要替换的确定替换对啊，比如各个数据包的名字和序列帧号名字，细心点）：
while（1）
{
发送与第一个基站交互的数据包
if (status_reg & SYS_STATUS_RXFCG)
{
与基站1 数据 交互处理
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
延时几毫秒；
发送与第二个基站交互的数据包
if (status_reg & SYS_STATUS_RXFCG)
{
与基站2 数据 交互处理 // 相应的发送接收数据包要记得替换，还有序列帧号
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
延时几毫秒；
发送与第三个基站交互的数据包
if (status_reg & SYS_STATUS_RXFCG)
{
与基站3 数据 交互处理// 相应的发送接收数据包要记得替换，还有序列帧号
}
else
{
/* Clear RX error events in the DW1000 status register. */
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_ALL_RX_ERR);
}
延时几毫秒；
}检查无误的话，这样其实1标签与3基站的测距已经完成了，可以自行将各个分基站接收的数据输出显示一下。接下来是将我们分基站2和3号收到的数据立马发送给总基站1。
如下：
之前我们已经定义好了基站与基站之前发送的数据包（其实和标签与基站的原理一样，只不过基站与基站更简单而已，类似于ss测距方法）
分基站 要做的就是解析到数据将距离数据装入数据包中，发送出去
sprintf(dist_str, "DIST1#%3.2f#m ", distance);
USART_putstr(dist_str);
USART_putc('\n');
for(i=10;i<18;i++)
{
tx_final_msg_station2[i] = dist_str[i-4];
}
tx_final_msg_station2[ALL_MSG_SN_IDX] = frame_seq_nb_tx_station2;
dwt_writetxdata(sizeof(tx_final_msg_station2), tx_final_msg_station2, 0);
dwt_writetxfctrl(sizeof(tx_final_msg_station2), 0);
/* Start transmission. */
dwt_starttx(DWT_START_TX_IMMEDIATE);
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)){};
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);总基站就负责接收（框架如下）：
if (status_reg & SYS_STATUS_RXFCG)
{
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);
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;
if(distance <= 0.00) distance = 0.00;
/* Display computed distance on LCD. */
sprintf(dist_str, "DIST2@%3.2f@m ", distance);
USART_putstr(dist_str);
USART_putc('\n');
//lcd_display_str(dist_str);
}
else if(memcmp(rx_buffer, rx_station2_msg, ALL_MSG_COMMON_LEN) == 0)
{
//USART_putstr("rec data from station2 :\n");
for(i=6;i<14;i++)
{
dist_str_rx_station2[i] = rx_buffer[i+4];
}
USART_putstr(dist_str_rx_station2);
USART_putc('\n');
}
else if(memcmp(rx_buffer, rx_station3_msg, ALL_MSG_COMMON_LEN) == 0)
{
//USART_putstr("rec data from station3 :\n");
for(i=6;i<14;i++)
{
dist_str_rx_station3[i] = rx_buffer[i+4];
}
USART_putstr(dist_str_rx_station3);
USART_putc('\n');
}
}之前没问题的话，然后将总基站数据发送到串口调试助手上，我们就会看到实时刷新的标签到3基站的距离，至此，目的完成。