[開源項目]藍點無限TWR算法-多基站多標簽固件
初衷：
隨著UWB大力發展，國內實際應用逐步落地。 對于UWB的需求已經不是停留在實驗測試階段，
目前逐步進入商用大環境，很多廠商特殊需要一定要選用TWR，無法接收TDOA。
但是受限于目前大部分TWR方案一般只能支持3-4基站定位，很多廠商無法實現UWB項目快速落地。
基于以上，我們打算開源一套多基站多標簽固件，方便高需求客戶見解和二次開發。
同時，對于學習測試的客戶，建議依然使用基本版本3-4基站定位，這種多基站多標簽實現增加了更多邏輯部分的實現，不適合入門學習。
軟件流程：
關鍵流程說明：
除了完成基本測距以外，這里主要需要完成動態識別功能，標簽需要動態識別它周圍的基站，時刻可以保持與周圍3-4個基站進行測距。
固件Base：
這個固件依然基于我們的開源框架項目51uwb_base進行二次開發，
更多開源框架相關參見鏈接：http://51uwb.cn/forum.php?mod=viewthread&tid=165&extra=page%3D1
固件匹配Python開源上位機：
上位機使用我們目前已經開源的純python版本上位機，相關鏈接：http://51uwb.cn/forum.php?mod=viewthread&tid=401&extra=page%3D1
固件重要問題說明：
1 串口輸出，不同以往，這里用的是標簽串口
2 上位機接口目前是TCP，需要使用串口轉TCP工具:串口轉TCP參見視頻https://www.bilibili.com/video/BV1rv411p7Hj/
固件源碼：
固件源碼已經放到git上，V1.0 版本開發完成，請詳細看下面的描述
https://tuzhuke@bitbucket.org/tuzhuke/bp30_multianthor.git
固件開發記錄
Day1：
githash:a387f5cdbbff3b6b1c818eaf459b4ad2a6fe24c0
主要完成功能，標簽發送廣播信號，基站接收廣播信號。標簽發送的廣播信號需要包含已經識別的基站地址。
Step1 在標簽中定義一個存放基站的結構體數組
struct Anthor_Information
{
uint16 short_address;//基站16bit 短地址
uint16 distance;//距離信息，高8 低8bit
uint32 last_time;//上次通信時間
uint8 rssi_info;//上次通信RSSI值記錄
unsigned char alive; //是否已經識別或者是否已經丟失
} anthor_info[MAX_ANTHORStep2 在標簽中發送廣播信號，廣播信號包含了已經識別的基站，如果基站收到這個信息，發現數據包中已經有自己地址就無需反饋，否則反饋信息給標簽。函數試下如下：
點擊查看代碼
/*******************************************************************************
* 函數名 : BPhero_TAG_Broadcast
* 描述 : 標簽啟動發送廣播信息給各個基站,信息數據包包括了基站短地址
* 輸入 : 無
* 輸出 : 無
* 返回值 : 無
* 說明 : 發送broadcast信息(B信息)給所有基站
*******************************************************************************/
void BPhero_TAG_Broadcast(void)
{
uint8 index = 0 ;
uint8 strlen = 0;
msg_f_send.destAddr[0] = 0xFF;
msg_f_send.destAddr[1] = 0xFF;
msg_f_send.seqNum = distance_seqnum;
msg_f_send.messageData[0]='B';//broadcast message
strlen = strlen + 1;
uint8 *pAnthor_Str = &msg_f_send.messageData[1];
//后面跟基站信息
for(index = 0 ; index < MAX_ANTHOR; index++)
{
if(anthor_info[index].alive == 1)
{
sprintf(pAnthor_Str, "%04X:",anthor_info[index].short_address);
pAnthor_Str = pAnthor_Str + 5;
strlen = strlen + 5;
}
}
//GPIOB.5設定，兼容之前帶PA的模塊-->如需求請聯系www.51uwb.cn
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
//寫入數據
dwt_writetxdata(11 + strlen,(uint8 *)&msg_f_send, 0) ; // write the frame data
dwt_writetxfctrl(11 + strlen, 0);
dwt_starttx(DWT_START_TX_IMMEDIATE); //啟動發送
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)) //等待發送完成
{ };
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除發送完成標志
poll_tx_ts = get_tx_timestamp_u64();//讀取發送時間戳
//清空接收緩存，待收到數據時使用
for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
{
rx_buffer[i] = '\0';
}
dwt_enableframefilter(DWT_FF_DATA_EN); //啟動幀過濾功能
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*10);//設定接收延時函數
dwt_rxenable(0);//啟動接收機
//sequence控制
if(++distance_seqnum == 255)
distance_seqnum = 0;
}Step3 在標簽中調用上述函數廣播發送，我們使用之前代碼定時器回調函數，通過回調函數，可以周期性發送
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
if (htim->Instance == htim3.Instance)
{
HAL_TIM_Base_Stop(&htim3);
{
dwt_forcetrxoff();
TAG_SendOut_Messge();
BPhero_TAG_Broadcast();
}
HAL_TIM_Base_Start(&htim3);
}
}Step4： 在基站接收broadcast “B”信號，在rx_main.c中收到“B信號”后打印一串字符。
case 'B':
printf("receive B message\n");
break;分別編譯標簽和基站的進行測試。串口數據如下圖，表明數據可以正常收到。
Day2:
githash: e23e4e1e3bf681d1125036e5dbbf5a07fe363fdc
主要完成基站收到B信息后，以R信息回復給標簽，標簽收到信息提取短地址，并更新自己的結構體數組
Step1 修改標簽廣播格式，在數據包中增加已知基站的個數。
//后面跟基站信息
for(index = 0 ; index < MAX_ANTHOR; index++)
{
if(anthor_info[index].alive == 1)
{
sprintf(pAnthor_Str, "%04X:",anthor_info[index].short_address);
pAnthor_Str = pAnthor_Str + 5;
strlen = strlen + 5;
anthor_count++;
}
}
msg_f_send.messageData[1] = anthor_count;Step2: 修改基站接收處理，目前只簡單反饋信息給標簽，以“R”信息回復到標簽，同時將標簽數據包中的“現有基站個數”打印出來用于debug
case 'B':
printf("receive B anthor = %d\n",msg_f->messageData[1]);
{
msg_f_send.messageData[0]='R';//Poll message
//后面修改這個數據長度
dwt_writetxdata(11 + 1, (uint8 *)&msg_f_send, 0) ; // write the frame data
dwt_writetxfctrl(11 + 1, 0);
dwt_starttx(DWT_START_TX_IMMEDIATE);
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
}
break;Step3: 標簽收到信息，提取基站信息中的地址
case 'R':
address = msg_f_recv->sourceAddr[1]<<8|msg_f_recv->sourceAddr[0];
printf("receive R message 0x%04X\n",address);
Update_Anthor_Info(address);
break; uint8 Update_Anthor_Info(uint32 shortaddress)
{
uint8 index = 0;
printf("shortaddress = 0x%04X\n",shortaddress);
//后面跟基站信息
for(index = 0 ; index < MAX_ANTHOR; index++)
{
if(anthor_info[index].alive == 0)
{
anthor_info[index].short_address = shortaddress;
anthor_info[index].alive = 1;
return 1;
}
}
return 0;
}Day3:
githash:4d1b64584706426c2a71174d7026cbe7696f2a4b
今天完成了基本功能開發，可以作為V1.0版本。
主要開發內容：基站解析標簽發送的廣播B信號，標簽匯總R信號基站，如果收到R基站大于等于4個開始測距，如果測距的時候發現基站丟失，重新啟動廣播B信號。
1 基站解析標簽廣播B信號，匹配是否有自己的地址，有地址忽略，沒有地址回復R信號
點擊查看代碼
case 'B':
printf("receive B anthor = %d\n",msg_f->messageData[1]);
Num_Anthor = msg_f->messageData[1];
Sourceaddress = msg_f->sourceAddr[1]<<8| msg_f->sourceAddr[0];
pAnthor_Str = &msg_f->messageData[2];
match_flag = 0;
for (Index = 0; Index < Num_Anthor; Index++)
{
printf("receive address = %04X\n",(pAnthor_Str[1]<<8|pAnthor_Str[0]));
if(SHORT_ADDR == (pAnthor_Str[1]<<8|pAnthor_Str[0])) //匹配成功
{
printf("match\n");
match_flag = 1;
}
pAnthor_Str = pAnthor_Str +3 ;
}
if(match_flag == 0)//沒有匹配到，發送一個反饋信息
{
msg_f_send.messageData[0]='R';//Poll message
//后面修改這個數據長度
dwt_writetxdata(11 + 1, (uint8 *)&msg_f_send, 0) ; // write the frame data
dwt_writetxfctrl(11 + 1, 0);
dwt_starttx(DWT_START_TX_IMMEDIATE);
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
}
break;2 標簽匯總基站反饋的R信號，其實這部分代碼在day2 已經完成，無需修改。
3 判斷收到R信號個數，這個在定時廣播里判斷的。如果小于4，持續執行廣播收集。
HAL_TIM_Base_Stop(&htim3);
{
dwt_forcetrxoff();
if(Count_Anthor() < 4)
{
gProcess_Dis = 0;
BPhero_TAG_Broadcast();
gSend_index = 0;
}具體實現函數
uint8 Count_Anthor()
{
uint8 index = 0;
uint8 count = 0;
//后面跟基站信息
for(index = 0 ; index < MAX_ANTHOR; index++)
{
if(anthor_info[index].alive == 1)
{
count++;
}
}
return count;
}3 當R信號基站數量等于4個，開始啟動測距
點擊查看代碼
if(Count_Anthor() < 4)
{
gProcess_Dis = 0;
BPhero_TAG_Broadcast();
gSend_index = 0;
}
else
{
if(gSend_index ==Count_Anthor())
{
gSend_index= 0;
Send_Dis_To_Anthor0();
} else
{
gProcess_Dis = 1;
BPhero_Distance_Measure_Specail_ANTHOR();// 從1 2 3 4發送
}
}這里的Send_Dis_To_Anthor0()是沿用之前的函數名，其實在這個里面實現了數據格式組裝并在串口打印，以及調用函數在液晶顯示。
BPhero_Distance_Measure_Specail_ANTHOR()主要功能就是啟動測距，測距對象是收集到R信號的基站。
點擊查看代碼
void BPhero_Distance_Measure_Specail_ANTHOR(void)
{
uint16 destaddress = Find_Address();
// printf("Send Index = %d, Address = 0x%04X\n",gSend_index,destaddress);
msg_f_send.destAddr[0] =(destaddress) &0xFF;
msg_f_send.destAddr[1] = ((destaddress)>>8) &0xFF;
msg_f_send.seqNum = distance_seqnum;
msg_f_send.messageData[0]='P';//Poll message
//GPIOB.5設定，兼容之前帶PA的模塊-->如需求請聯系www.51uwb.cn
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
//寫入數據
dwt_writetxdata(12,(uint8 *)&msg_f_send, 0) ; // write the frame data
dwt_writetxfctrl(12, 0);
dwt_starttx(DWT_START_TX_IMMEDIATE); //啟動發送
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS)) //等待發送完成
{ };
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除發送完成標志
poll_tx_ts = get_tx_timestamp_u64();//讀取發送時間戳
//清空接收緩存，待收到數據時使用
for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
{
rx_buffer[i] = '\0';
}
dwt_enableframefilter(DWT_FF_DATA_EN); //啟動幀過濾功能
dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);//設定接收延時函數
dwt_rxenable(0);//啟動接收機
//sequence控制
if(++distance_seqnum == 255)
distance_seqnum = 0;
}4 中斷回調函數中處理timeout，如果測距對象基站沒有反饋，標簽發生timeout中斷，則立即將該基站islive 設置為0，帶下次統計，發現基站數量小于4，則標簽重新發送廣播信號收集基站
else
{
if(gProcess_Dis == 1)
{
printf("timeout address 0x%04X\n",Find_Address());
Delete_Anthor(Find_Address());
}其它更新：
git hash:4892896b3d09e6009dfbe3d537e866f2c94d2d36
修改了一個祖傳代碼bug，使用了野指針....
git hash：d2e01e118126c9ce9c84337126db3e92d23ed3ba
修改了UWB中斷，讓UWB中斷只處理接收成功和timeout兩種事件，其他事件均不處理
同時，調試的時候發現上位機當收到異常數據無法處理，導致異常退出
def twr_main(input_string):
print(input_string)
error_flag, result_dic = Process_String_Before_Udp(input_string)
if error_flag == 0:
[location_result, location_seq, location_addr, location_x, location_y] = Compute_Location(result_dic)
return location_result, location_seq, location_addr, location_x, location_y
return 0, 0, 0, 0, 0
當發生異常，增加return 0, 0, 0, 0, 0，代碼同步更新到上位機部分鏈接。
自此，通過三天，零散的時間，開發出一套可以動態識別基站并完成測距的固件代碼，代碼編寫時間和測試debug時間基本是1:5，更多的是細節考慮步驟導致異常.
開發過程中遇到一個很詭異的異常，當其中一個基站地址將短地址設置為0x0006后，標簽識別成功但是無法完成測距，通過加debug信息最終發現是由于標簽測距完成后有一個濾波器，濾波器設置的最大基站數目為5，導致數組越界訪問。
三天零散的開發時間，可以說這次開發非常順利，一是由于有一個比較完備的代碼框架，基于之前的代碼框架開發，可以減少對于底層的依賴，只需要實現邏輯部分即可。而邏輯部分其實在很早之前就有想法，通過想法落實到流程圖，規劃每一步要做什么。目前的代碼還沒有切合到流程圖上，流程圖中，我的想法是即便有4個基站可以定位，依然周期性的發送廣播，發現更多的基站，選取附近的基站做參考。 由于時間關系，這個部分可能留給各位看官了。
關于硬件，目前我們的代碼，基于硬件是我們自研的BP30，使用主控是F4。同時可以無縫在BP400 上使用。 如果沒有我們的硬件，可以適當進行移植，匹配主控。
最后，歡迎交流分享！
浙公網安備 33010602011771號