51uwb.cn

标题: UWB DWM1000 开源项目框架 之 TWR定位实现 [打印本页]
作者: tuzhuke    时间: 2020-4-27 08:06
标题: UWB DWM1000 开源项目框架 之 TWR定位实现
对于使用三边定位原理实现的算法而言，定位实质就是一个三对一测距(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;

  7.     htim3.Instance = TIM3;
  8.     htim3.Init.Prescaler = 8400-1;
  9.     htim3.Init.CounterMode = TIM_COUNTERMODE_UP;
  10.     htim3.Init.Period = 500-1;
  11.     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;

  7.     msg_f_send.seqNum = distance_seqnum;
  8.     msg_f_send.messageData[0]='P';//Poll message
  9.     HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);
  10.     dwt_writetxdata(psduLength,(uint8 *)&msg_f_send, 0) ;  // write the frame data
  11.     dwt_writetxfctrl(psduLength, 0);
  12.     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);

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


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





作者: CHEN34    时间: 2020-10-12 11:54
感谢楼主的分享，谢谢！
作者: 藏丶宝    时间: 2021-5-11 17:38
请问  51uwb 定位框架-高频率高容量测试代码 这个源码怎么获得
作者: szmcumcu    时间: 2021-9-30 17:01
这个资料不错，学习了，谢谢
作者: 760371303    时间: 2021-10-15 01:29
感谢大佬分享
作者: adviil    时间: 2021-11-15 11:08
感谢分享，学习一下~
作者: yuhao    时间: 2022-5-1 16:39
感谢楼主的分享，谢谢
作者: 馒头卡    时间: 2022-10-3 16:29
感谢分享，好好研究下
作者: 大北风    时间: 2022-10-9 17:21
感谢分析，资料很棒
作者: hutpond    时间: 2022-11-25 10:55
学习了，谢谢分享。
作者: wsy    时间: 2023-4-29 10:47
感谢楼主分享
作者: 550880768    时间: 2023-8-31 16:10
感谢大佬分享！
作者: 凤凰院凶真1986    时间: 2024-2-14 23:01

学习了，谢谢分享。
作者: emm_6661    时间: 5 天前
凤凰院凶真1986 发表于 2024-2-14 23:01
学习了，谢谢分享。

哥学会没，有点问题想咨询救救我哥



欢迎光临 51uwb.cn (http://51uwb.cn/) Powered by Discuz! X3.3