【开源项目】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

兔子 · 发表于 2021-9-11 23:11:21

可以简单说说“很多厂商特殊需要一定要选用TWR，无法接收TDOA。”的原因吗楼主~

蓝点无限 · 发表于 2021-9-12 11:00:12

兔子发表于 2021-9-11 23:11
可以简单说说“很多厂商特殊需要一定要选用TWR，无法接收TDOA。”的原因吗楼主~

接触过做煤矿项目的工程师，他们有个要求，单个基站存在的情况的时候，要获取到标签和这个基站的距离。TDOA这个做不到

sdfb6868 · 发表于 2021-9-13 02:54:46

蓝点无限发表于 2021-9-12 11:00
接触过做煤矿项目的工程师，他们有个要求，单个基站存在的情况的时候，要获取到标签和这个基站的距离。TD ...

TDOA只解决了“他在哪”不能解决“我在哪”的问题。标签单向广播，无法反向给标签传位置信息，使得基站知道标签在哪里，而标签自己不知道在哪里，应用到无人平台后无法解决“我在哪”的问题。TDOA不适用于此类场景

bingnuo · 发表于 2021-9-13 11:40:52

使用你们的模块与代码怎么标签出现重启现象呢，

蓝点无限 · 发表于 2021-9-13 22:00:22

sdfb6868 发表于 2021-9-13 02:54
TDOA只解决了“他在哪”不能解决“我在哪”的问题。标签单向广播，无法反向给标签传位置信息，使得基站知 ...

很形象，你说的这个是上行模式，其实下行模式可以解决“我在哪”的问题，下行模式就是GPS了，不过标签功耗较大，计算量也大了。

蓝点无限 · 发表于 2021-9-13 22:01:35

bingnuo 发表于 2021-9-13 11:40
使用你们的模块与代码怎么标签出现重启现象呢，

详细描述，或者提供一个完整的视频。
视频可以上传到B站，直接回复在论坛，论坛即可查看视频

szmcumcu · 发表于 2021-9-28 22:38:05

好资料，可惜下载不了，谢谢楼主的无私分享

兔子 · 发表于 2021-10-5 12:55:25

我的电脑编译的时候会提示缺少i2c_sw.h 需要手动在魔术棒的C/C++添加platform中的mpu9250到编译路径中

兔子 · 发表于 2021-10-5 13:56:29

蓝点无限发表于 2021-9-13 22:01
详细描述，或者提供一个完整的视频。
视频可以上传到B站，直接回复在论坛，论坛即可查看视频

我现在用STLINK下载好固件后串口都打印乱码然后标签双红灯不闪烁搜索基站功能失效

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