UWB定位 - DWM1000模組除錯簡單心得 - 3
前倆篇介紹了簡單的一基站一標籤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[] = {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};
然後就是在總基站1程式碼中多重新定義倆個幀序列號:
//總基站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)
{
與基站3 資料 互動處理 // 相應的傳送接收資料包要記得替換,還有序列幀號
}
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)
{
基站1 資料 互動處理// 相應的傳送接收資料包要記得替換,還有序列幀號
}
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[i] = dist_str[i-4];
}
tx_final_msg[ALL_MSG_SN_IDX] = frame_seq_nb_tx_station1;
dwt_writetxdata(sizeof(tx_final_msg), tx_final_msg, 0);
dwt_writetxfctrl(sizeof(tx_final_msg), 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)
{
處理與標籤的資料互動;
}
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基站的距離,至此,目的完成。
未完待續。。。