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基站的距離，至此，目的完成。

?

?

總結

以上是生活随笔為你收集整理的【UWB定位】 - DWM1000模块调试简单心得 - 3的全部內容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網站內容還不錯，歡迎將生活随笔推薦給好友。