51uwb.cn

 找回密码
 立即注册
查看: 8565|回复: 12

UWB DWM1000 开源项目框架 之 TWR定位实现

[复制链接]

73

主题

249

帖子

6726

积分

管理员

Rank: 9Rank: 9Rank: 9

积分
6726
发表于 2020-4-27 08:06:41 | 显示全部楼层 |阅读模式
对于使用三边定位原理实现的算法而言,定位实质就是一个三对一测距(2D),在之前帖子中已经贴出了如何实现测距
http://51uwb.cn/forum.php?mod=viewthread&tid=188&extra=page%3D1

这里简单实现TWR定位,这个代码在之前测距基础上除了实现三对一测距以外,还增加了对于PA的支持(目前我们这里有一款模块,增加了PA,距离可以达到400-500m)。

具体上代码
1 定时器周期发送测距请求,在定时器的回调函数中调用BPhero_Distance_Measure_Specail_TAG();
  1. void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
  2. {
  3.     if (htim->Instance == htim3.Instance)
  4.     {
  5.         /* Toggle LED */
  6.         HAL_GPIO_TogglePin(GPIOA, GPIO_PIN_0|GPIO_PIN_1|GPIO_PIN_2|GPIO_PIN_3|GPIO_PIN_4);
  7.         dwt_forcetrxoff();
  8.         BPhero_Distance_Measure_Specail_TAG();
  9.         HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_8);
  10.     }
  11. }
复制代码
用定时器周期调用测距请求,在调频率的时候就非常好调了,只需要改如下标红的8400即可
  1. TIM_HandleTypeDef  htim3;
  2. static void MX_TIM3_Init(void)
  3. {
  4.     TIM_ClockConfigTypeDef sClockSourceConfig;
  5.     TIM_MasterConfigTypeDef sMasterConfig;

  6.     htim3.Instance = TIM3;
  7.     htim3.Init.Prescaler = 8400-1;
  8.     htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  9.     htim3.Init.Period = 500-1;
  10.     htim3.Init.ClockDivision = TIM_CLOCKDIVISION_DIV1;
复制代码


在发送请求部分,轮训三个地址,0x0001 到 0x0003,基站必须配置为这三个地址
  1. uint16 Dest_Address =  0x0000;
  2. void BPhero_Distance_Measure_Specail_TAG(void)
  3. {
  4.     msg_f_send.destAddr[0] =(Dest_Address+1) &0xFF;
  5.     msg_f_send.destAddr[1] =  ((Dest_Address+1)>>8) &0xFF;

  6.     msg_f_send.seqNum = distance_seqnum;
  7.     msg_f_send.messageData[0]='P';//Poll message
  8.     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);
  9.     dwt_writetxdata(psduLength,(uint8 *)&msg_f_send, 0) ;  // write the frame data
  10.     dwt_writetxfctrl(psduLength, 0);
  11.     dwt_starttx(DWT_START_TX_IMMEDIATE);
复制代码


其他部分大体与TWR测距一致,特别一点,我们这里增加了kalman滤波,在最后计算出距离之后用kalman滤波
  1. distance = tof * SPEED_OF_LIGHT;
  2.                     distance = distance - dwt_getrangebias(config.chan,(float)distance, config.prf);//距离减去矫正系数
  3.                     //kalman filter
  4.                     distance = KalMan(distance);

  5.                     //send distance message to tag
  6.                     int temp = (int)(distance*100);
复制代码


更多代码请直接拉git上的完整工程吧,欢迎一起讨论
  1. https://bitbucket.org/tuzhuke/twr_location_v1/src/master/
复制代码





回复

使用道具 举报

0

主题

5

帖子

5

积分

新手上路

Rank: 1

积分
5
发表于 2020-10-12 11:54:45 | 显示全部楼层
感谢楼主的分享,谢谢!
回复

使用道具 举报

0

主题

29

帖子

30

积分

新手上路

Rank: 1

积分
30
发表于 2021-5-11 17:38:15 | 显示全部楼层
请问  51uwb 定位框架-高频率高容量测试代码 这个源码怎么获得
回复

使用道具 举报

0

主题

45

帖子

48

积分

新手上路

Rank: 1

积分
48
发表于 2021-9-30 17:01:50 | 显示全部楼层
这个资料不错,学习了,谢谢
回复

使用道具 举报

1

主题

7

帖子

17

积分

新手上路

Rank: 1

积分
17
发表于 2021-10-15 01:29:44 | 显示全部楼层
感谢大佬分享
回复

使用道具 举报

1

主题

13

帖子

32

积分

游客

积分
32
发表于 2021-11-15 11:08:24 | 显示全部楼层
感谢分享,学习一下~
回复

使用道具 举报

0

主题

5

帖子

6

积分

新手上路

Rank: 1

积分
6
发表于 2022-5-1 16:39:07 | 显示全部楼层
感谢楼主的分享,谢谢
回复

使用道具 举报

0

主题

6

帖子

12

积分

新手上路

Rank: 1

积分
12
发表于 2022-10-3 16:29:40 | 显示全部楼层
感谢分享,好好研究下
回复

使用道具 举报

0

主题

6

帖子

10

积分

新手上路

Rank: 1

积分
10
发表于 2022-10-9 17:21:49 | 显示全部楼层
感谢分析,资料很棒
回复

使用道具 举报

0

主题

1

帖子

12

积分

新手上路

Rank: 1

积分
12
发表于 2022-11-25 10:55:00 | 显示全部楼层
学习了,谢谢分享。
回复

使用道具 举报

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

本版积分规则

bphero Inc.  

GMT+8, 2024-3-29 05:44 , Processed in 0.017136 second(s), 3 queries , File On.

Powered by Discuz! X3.3

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

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