【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_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

Nginx R31 doc 官方文檔-01-nginx 如何安裝

Qt/C++音視頻開發74-合併標籤圖形/生成yolo運算結果圖形/文字和圖形合併成一個/水印濾鏡

挑戰程序設計競賽 2.2章習題 POJ - 3617 Best Cow Line 貪心

字節面試：MySQL什麼時候鎖表？如何防止鎖表？

.NET8連接SQL SERVER 2008 R2 報：證書鏈是由不受信任的頒發機構頒發的

golang開發環境搭建(win10)

python計算機視覺學習筆記——PIL庫的用法

【QT】信號槽傳遞參數技巧

【QT error】 The name 'layoutWidget' (QWidget) is already in use, defaulting to 'layoutWidget1'.

【QT】QT主界面卡死

【Qt】QtConCurrent::run調用成員函數

realtek 8111E 網卡修改MAC 地址

https://yachay.unat.edu.pe/blog/index.php?comment_area=format_blog&comment_component=blog&comment_co

linux以太網驅動總結