51uwb.cn

 找回密码
 立即注册
查看: 17331|回复: 85

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

  [复制链接]

35

主题

941

帖子

3910

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
3910
发表于 2021-8-17 07:59:19 | 显示全部楼层 |阅读模式

初衷:           
       随着UWB大力发展,国内实际应用逐步落地。 对于UWB的需求已经不是停留在实验测试阶段,
       目前逐步进入商用大环境,很多厂商特殊需要一定要选用TWR,无法接收TDOA。
       但是受限于目前大部分TWR方案一般只能支持3-4基站定位,很多厂商无法实现UWB项目快速落地。
       基于以上,我们打算开源一套多基站多标签固件,方便高需求客户见解和二次开发。
       同时,对于学习测试的客户,建议依然使用基本版本3-4基站定位,这种多基站多标签实现增加了更多逻辑部分的实现,不适合入门学习。

软件流程:
twr_location.png

关键流程说明:
除了完成基本测距以外,这里主要需要完成动态识别功能,标签需要动态识别它周围的基站,时刻可以保持与周围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 在标签中定义一个存放基站的结构体数组
  1. struct Anthor_Information
  2. {
  3.      uint16 short_address;//基站16bit 短地址
  4.      uint16 distance;//距离信息,高8 低8bit
  5.      uint32 last_time;//上次通信时间
  6.      uint8  rssi_info;//上次通信RSSI值记录
  7.     unsigned char alive; //是否已经识别或者是否已经丢失
  8. } anthor_info[MAX_ANTHOR];
复制代码
Step2 在标签中发送广播信号,广播信号包含了已经识别的基站,如果基站收到这个信息,发现数据包中已经有自己地址就无需反馈,否则反馈信息给标签。函数试下如下:
  1. /*******************************************************************************
  2. * 函数名  : BPhero_TAG_Broadcast
  3. * 描述    : 标签启动发送广播信息给各个基站,信息数据包包括了基站短地址
  4. * 输入    : 无
  5. * 输出    : 无
  6. * 返回值  : 无
  7. * 说明    : 发送broadcast信息(B信息)给所有基站
  8. *******************************************************************************/

  9. void BPhero_TAG_Broadcast(void)
  10. {
  11.     uint8 index = 0 ;
  12.     uint8 strlen = 0;
  13.     msg_f_send.destAddr[0] = 0xFF;
  14.     msg_f_send.destAddr[1] = 0xFF;

  15.     msg_f_send.seqNum = distance_seqnum;
  16.     msg_f_send.messageData[0]='B';//broadcast message
  17.     strlen = strlen + 1;

  18.     uint8 *pAnthor_Str = &msg_f_send.messageData[1];
  19.     //后面跟基站信息
  20.     for(index = 0 ; index < MAX_ANTHOR; index++)
  21.     {
  22.         if(anthor_info[index].alive == 1)
  23.         {
  24.             sprintf(pAnthor_Str, "%04X:",anthor_info[index].short_address);
  25.             pAnthor_Str = pAnthor_Str + 5;
  26.             strlen = strlen + 5;
  27.         }
  28.     }

  29.     //GPIOB.5设定,兼容之前带PA的模块-->如需求请联系www.51uwb.cn
  30.     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
  31.     //写入数据
  32.     dwt_writetxdata(11 + strlen,(uint8 *)&msg_f_send, 0) ;  // write the frame data
  33.     dwt_writetxfctrl(11 + strlen, 0);
  34.     dwt_starttx(DWT_START_TX_IMMEDIATE);        //启动发送
  35.     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))        //等待发送完成
  36.     { };
  37.     dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除发送完成标志
  38.     poll_tx_ts = get_tx_timestamp_u64();//读取发送时间戳

  39.     //清空接收缓存,待收到数据时使用
  40.     for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
  41.     {
  42.         rx_buffer[i] = '\0';
  43.     }
  44.     dwt_enableframefilter(DWT_FF_DATA_EN);        //启动帧过滤功能
  45.     dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*10);//设定接收延时函数
  46.     dwt_rxenable(0);//启动接收机
  47.     //sequence控制
  48.     if(++distance_seqnum == 255)
  49.         distance_seqnum = 0;
  50. }
复制代码
Step3 在标签中调用上述函数广播发送,我们使用之前代码定时器回调函数,通过回调函数,可以周期性发送
  1. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  2. {
  3.     if (htim->Instance == htim3.Instance)
  4.     {
  5.         HAL_TIM_Base_Stop(&htim3);
  6.         {
  7.             dwt_forcetrxoff();
  8.             TAG_SendOut_Messge();
  9.             BPhero_TAG_Broadcast();
  10.         }

  11.         HAL_TIM_Base_Start(&htim3);
  12.     }
  13. }
复制代码
Step4: 在基站接收broadcast “B”信号,在rx_main.c中收到“B信号”后打印一串字符。
  1.             case 'B':
  2.                 printf("receive B message\n");
  3.                 break;
复制代码
分别编译标签和基站的进行测试。串口数据如下图,表明数据可以正常收到。
Image 1.png

Day2:
githash: e23e4e1e3bf681d1125036e5dbbf5a07fe363fdc

主要完成基站收到B信息后,以R信息回复给标签,标签收到信息提取短地址,并更新自己的结构体数组

Step1 修改标签广播格式,在数据包中增加已知基站的个数。
  1.     //后面跟基站信息
  2.     for(index = 0 ; index < MAX_ANTHOR; index++)
  3.     {
  4.         if(anthor_info[index].alive == 1)
  5.         {
  6.             sprintf(pAnthor_Str, "%04X:",anthor_info[index].short_address);
  7.             pAnthor_Str = pAnthor_Str + 5;
  8.             strlen = strlen + 5;
  9.                                           anthor_count++;
  10.         }
  11.     }
  12.                 msg_f_send.messageData[1] = anthor_count;
复制代码
Step2: 修改基站接收处理,目前只简单反馈信息给标签,以“R”信息回复到标签,同时将标签数据包中的“现有基站个数”打印出来用于debug
  1.             case 'B':
  2.                 printf("receive B anthor = %d\n",msg_f->messageData[1]);
  3.                 {
  4.                     msg_f_send.messageData[0]='R';//Poll message
  5.                     //后面修改这个数据长度
  6.                     dwt_writetxdata(11 + 1, (uint8 *)&msg_f_send, 0) ; // write the frame data
  7.                     dwt_writetxfctrl(11 + 1, 0);
  8.                     dwt_starttx(DWT_START_TX_IMMEDIATE);
  9.                     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
  10.                     { };
  11.                 }
  12.                 break;
复制代码
Step3: 标签收到信息,提取基站信息中的地址
  1.    case 'R':
  2.   address = msg_f_recv->sourceAddr[1]<<8|msg_f_recv->sourceAddr[0];
  3.   printf("receive R message 0x%04X\n",address);
  4.   Update_Anthor_Info(address);
  5.   break;
复制代码
  1. uint8 Update_Anthor_Info(uint32 shortaddress)
  2. {
  3.    uint8 index = 0;
  4.    printf("shortaddress = 0x%04X\n",shortaddress);
  5.    //后面跟基站信息
  6.     for(index = 0 ; index < MAX_ANTHOR; index++)
  7.     {
  8.         if(anthor_info[index].alive == 0)
  9.         {
  10.                                         anthor_info[index].short_address = shortaddress;
  11.                                         anthor_info[index].alive = 1;        
  12.                                         return 1;
  13.         }
  14.     }
  15.                 return 0;
  16. }
复制代码
Day3:
githash:4d1b64584706426c2a71174d7026cbe7696f2a4b

今天完成了基本功能开发,可以作为V1.0版本。
主要开发内容:基站解析标签发送的广播B信号,标签汇总R信号基站,如果收到R基站大于等于4个开始测距,如果测距的时候发现基站丢失,重新启动广播B信号。
1 基站解析标签广播B信号,匹配是否有自己的地址,有地址忽略,没有地址回复R信号
  1.             case 'B':
  2.                 printf("receive B anthor = %d\n",msg_f->messageData[1]);
  3.                 Num_Anthor = msg_f->messageData[1];
  4.                 Sourceaddress =  msg_f->sourceAddr[1]<<8| msg_f->sourceAddr[0];
  5.                 pAnthor_Str = &msg_f->messageData[2];
  6.                 match_flag = 0;
  7.                 for (Index = 0; Index < Num_Anthor; Index++)
  8.                 {
  9.                     printf("receive address = %04X\n",(pAnthor_Str[1]<<8|pAnthor_Str[0]));

  10.                     if(SHORT_ADDR == (pAnthor_Str[1]<<8|pAnthor_Str[0])) //匹配成功
  11.                     {
  12.                         printf("match\n");
  13.                         match_flag = 1;

  14.                     }
  15.                     pAnthor_Str = pAnthor_Str +3 ;
  16.                 }

  17.                 if(match_flag == 0)//没有匹配到,发送一个反馈信息
  18.                 {
  19.                     msg_f_send.messageData[0]='R';//Poll message
  20.                     //后面修改这个数据长度
  21.                     dwt_writetxdata(11 + 1, (uint8 *)&msg_f_send, 0) ; // write the frame data
  22.                     dwt_writetxfctrl(11 + 1, 0);
  23.                     dwt_starttx(DWT_START_TX_IMMEDIATE);
  24.                     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
  25.                     { };
  26.                 }
  27.                 break;
复制代码
2 标签汇总基站反馈的R信号,其实这部分代码在day2 已经完成,无需修改。
3 判断收到R信号个数,这个在定时广播里判断的。如果小于4,持续执行广播收集。
  1.         HAL_TIM_Base_Stop(&htim3);
  2.         {
  3.             dwt_forcetrxoff();
  4.             if(Count_Anthor() < 4)
  5.             {
  6.                 gProcess_Dis = 0;
  7.                 BPhero_TAG_Broadcast();
  8.                 gSend_index = 0;

  9.             }
复制代码
具体实现函数
  1. uint8 Count_Anthor()
  2. {
  3.     uint8 index = 0;
  4.     uint8 count = 0;
  5.     //后面跟基站信息
  6.     for(index = 0 ; index < MAX_ANTHOR; index++)
  7.     {
  8.         if(anthor_info[index].alive == 1)
  9.         {
  10.             count++;
  11.         }
  12.     }
  13.     return count;
  14. }
复制代码

3 当R信号基站数量等于4个,开始启动测距
  1.             if(Count_Anthor() < 4)
  2.             {
  3.                 gProcess_Dis = 0;
  4.                 BPhero_TAG_Broadcast();
  5.                 gSend_index = 0;

  6.             }
  7.             else
  8.             {
  9.                 if(gSend_index ==Count_Anthor())
  10.                 {
  11.                     gSend_index= 0;
  12.                     Send_Dis_To_Anthor0();
  13.                 } else
  14.                 {
  15.                     gProcess_Dis = 1;
  16.                     BPhero_Distance_Measure_Specail_ANTHOR();// 从1 2 3 4发送
  17.                 }
  18.             }
复制代码
这里的Send_Dis_To_Anthor0()是沿用之前的函数名,其实在这个里面实现了数据格式组装并在串口打印,以及调用函数在液晶显示。
BPhero_Distance_Measure_Specail_ANTHOR()主要功能就是启动测距,测距对象是收集到R信号的基站。
  1. void BPhero_Distance_Measure_Specail_ANTHOR(void)
  2. {
  3.     uint16 destaddress = Find_Address();
  4.    // printf("Send Index = %d, Address = 0x%04X\n",gSend_index,destaddress);
  5.     msg_f_send.destAddr[0] =(destaddress) &0xFF;
  6.     msg_f_send.destAddr[1] =  ((destaddress)>>8) &0xFF;

  7.     msg_f_send.seqNum = distance_seqnum;
  8.     msg_f_send.messageData[0]='P';//Poll message

  9.     //GPIOB.5设定,兼容之前带PA的模块-->如需求请联系www.51uwb.cn
  10.     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
  11.     //写入数据
  12.     dwt_writetxdata(12,(uint8 *)&msg_f_send, 0) ;  // write the frame data
  13.     dwt_writetxfctrl(12, 0);
  14.     dwt_starttx(DWT_START_TX_IMMEDIATE);        //启动发送
  15.     while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))        //等待发送完成
  16.     { };
  17.     dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除发送完成标志
  18.     poll_tx_ts = get_tx_timestamp_u64();//读取发送时间戳

  19.     //清空接收缓存,待收到数据时使用
  20.     for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
  21.     {
  22.         rx_buffer[i] = '\0';
  23.     }
  24.     dwt_enableframefilter(DWT_FF_DATA_EN);        //启动帧过滤功能
  25.     dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);//设定接收延时函数
  26.     dwt_rxenable(0);//启动接收机
  27.     //sequence控制
  28.     if(++distance_seqnum == 255)
  29.         distance_seqnum = 0;
  30. }
复制代码
4 中断回调函数中处理timeout,如果测距对象基站没有反馈,标签发生timeout中断,则立即将该基站islive 设置为0,带下次统计,发现基站数量小于4,则标签重新发送广播信号收集基站。
  1.    else
  2.     {
  3.         if(gProcess_Dis == 1)
  4.         {
  5.             printf("timeout address 0x%04X\n",Find_Address());
  6.             Delete_Anthor(Find_Address());
  7.         }
复制代码
其它更新:
git hash:4892896b3d09e6009dfbe3d537e866f2c94d2d36

修改了一个祖传代码bug,使用了野指针....
git hash:d2e01e118126c9ce9c84337126db3e92d23ed3ba

修改了UWB中断,让UWB中断只处理接收成功和timeout两种事件,其他事件均不处理

同时,调试的时候发现上位机当收到异常数据无法处理,导致异常退出
  1. def twr_main(input_string):
  2.     print(input_string)
  3.     error_flag, result_dic = Process_String_Before_Udp(input_string)
  4.     if error_flag == 0:
  5.         [location_result, location_seq, location_addr, location_x, location_y] = Compute_Location(result_dic)
  6.         return location_result, location_seq, location_addr, location_x, location_y
  7.     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









回复

使用道具 举报

35

主题

76

帖子

197

积分

官方会员

Rank: 8Rank: 8

积分
197
发表于 2021-9-11 23:11:21 | 显示全部楼层
可以简单说说“很多厂商特殊需要一定要选用TWR,无法接收TDOA。”的原因吗楼主~
回复

使用道具 举报

35

主题

941

帖子

3910

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
3910
 楼主| 发表于 2021-9-12 11:00:12 | 显示全部楼层
兔子 发表于 2021-9-11 23:11
可以简单说说“很多厂商特殊需要一定要选用TWR,无法接收TDOA。”的原因吗楼主~

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

使用道具 举报

4

主题

43

帖子

115

积分

注册会员

Rank: 2

积分
115
发表于 2021-9-13 02:54:46 | 显示全部楼层
蓝点无限 发表于 2021-9-12 11:00
接触过做煤矿项目的工程师,他们有个要求,单个基站存在的情况的时候,要获取到标签和这个基站的距离。TD ...

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

使用道具 举报

0

主题

6

帖子

33

积分

新手上路

Rank: 1

积分
33
发表于 2021-9-13 11:40:52 | 显示全部楼层
使用你们的模块与代码怎么标签出现重启现象呢,
回复

使用道具 举报

35

主题

941

帖子

3910

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
3910
 楼主| 发表于 2021-9-13 22:00:22 | 显示全部楼层
sdfb6868 发表于 2021-9-13 02:54
TDOA只解决了“他在哪”不能解决“我在哪”的问题。标签单向广播,无法反向给标签传位置信息,使得基站知 ...

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

使用道具 举报

35

主题

941

帖子

3910

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
3910
 楼主| 发表于 2021-9-13 22:01:35 | 显示全部楼层
bingnuo 发表于 2021-9-13 11:40
使用你们的模块与代码怎么标签出现重启现象呢,

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

使用道具 举报

0

主题

45

帖子

48

积分

新手上路

Rank: 1

积分
48
发表于 2021-9-28 22:38:05 | 显示全部楼层
好资料,可惜下载不了,谢谢楼主的无私分享
回复

使用道具 举报

35

主题

76

帖子

197

积分

官方会员

Rank: 8Rank: 8

积分
197
发表于 2021-10-5 12:55:25 | 显示全部楼层
我的电脑编译的时候会提示缺少i2c_sw.h  需要手动在魔术棒的C/C++添加platform中的mpu9250到编译路径中
回复

使用道具 举报

35

主题

76

帖子

197

积分

官方会员

Rank: 8Rank: 8

积分
197
发表于 2021-10-5 13:56:29 | 显示全部楼层
蓝点无限 发表于 2021-9-13 22:01
详细描述,或者提供一个完整的视频。
视频可以上传到B站,直接回复在论坛,论坛即可查看视频

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

使用道具 举报

您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

bphero Inc.  

GMT+8, 2024-3-29 23:39 , Processed in 0.030189 second(s), 6 queries , File On.

Powered by Discuz! X3.3

© 2001-2017 Comsenz Inc. Template By 【未来科技】【 www.wekei.cn 】

快速回复 返回顶部 返回列表