【开源项目】TWR算法-多基站多标签固件

蓝点无限 · 发表于 2021-8-17 07:59:19

初衷：
   随着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参见视频

固件源码：
固件源码已经放到git上，V1.0 版本开发完成，请详细看下面的描述
网页链接：https://tuzhuke@bitbucket.org/tuzhuke/bp30_multianthor
git下载： git clone 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_ANTHOR];

复制代码

Step2 在标签中发送广播信号，广播信号包含了已经识别的基站，如果基站收到这个信息，发现数据包中已经有自己地址就无需反馈，否则反馈信息给标签。函数试下如下：

/*******************************************************************************
* 函数名 : 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 上使用。如果没有我们的硬件，可以适当进行移植，匹配主控。

最后，欢迎交流分享！

写在最后，重要提示！！
我们的开源项目，工程以git 管理，工程逐步迭代，逐步增加功能，使用代码请参考hash提交版本。
TWR算法-多基站多标签固件 V1.0 版本 GIT HASH为：d887386e8ed3d7ac978234c03e3eaf876a058631

怪怪爱玩 · 发表于 2024-8-4 17:23:02

玩起来容易不？

想起温柔沉默默 · 发表于 2024-7-19 17:43:03

感谢大佬分享

Sulayman · 发表于 2024-7-16 09:28:45

能不能凑够10金币...我想下载

Sulayman · 发表于 2024-7-16 09:28:17

太感谢了，写的真棒，好好研究一下~

cheng6336 · 发表于 2024-5-20 17:27:58

谢谢分享，学习一下

zhengwei913 · 发表于 2024-5-6 17:01:37

这个好资料，学习中，谢谢了

LJJ · 发表于 2024-4-28 17:46:25

本帖最后由 LJJ 于 2024-4-28 17:48 编辑

C:\Users\DZ\Pictures\QQ截图20240428174313.png
我按照固件修改BPhero_Distance_Measure_Specail_ANTHOR函数，但是当断开将某个基站断电时，标签会一直向这个基站地址发送P信号，且无法判断到接收超时，这是为什么，求解答，谢谢

catliker · 发表于 2024-4-22 14:33:24

要怎么才能赚到积分啊？软件下不下来

吱吱吱 · 发表于 2024-4-19 09:08:40

好资料感谢分享

		自动登录	找回密码
密码			立即注册