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

tuzhuke

对于使用三边定位原理实现的算法而言，定位实质就是一个三对一测距(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. 
   
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;
   
6. 
   
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);
   
5. 
   
6.                     //send distance message to tag
   
7.                     int temp = (int)(distance*100);
   

复制代码

更多代码请直接拉git上的完整工程吧，欢迎一起讨论

1. https://bitbucket.org/tuzhuke/twr_location_v1/src/master/

复制代码

ldwx

TWR和TDOA哪种应用在工业环境中更适合？

chinatangk

TWR定位与TDOA定位哪个效果好些啊？

bp_test

感谢分析，资料很棒

Spiderman

学习了，谢谢分享。

emm_6661

凤凰院凶真1986 发表于 2024-2-14 23:01
学习了，谢谢分享。

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

凤凰院凶真1986

学习了，谢谢分享。

550880768

感谢大佬分享！

wsy

感谢楼主分享

hutpond

学习了，谢谢分享。

大北风

感谢分析，资料很棒