在前面两个博文中已经提及到,我们打算做一个UWB 结合运动传感器 融合定位,这篇博文实现固件代码,代码git 链接参见本文末尾。
我们的固件实现基础是之前的《TWR算法-多基站多标签固件》,参考链接
https://www.cnblogs.com/tuzhuke/p/15170219.html
再此基础上将《UWB 定位数据融合 之 MPU9250 Wake-on-Motion 模式》 MPU9250 相关内容进行整合。
具体改动
1. 将MPU9250 相关驱动和初始化移植
2 定义全局变量isMpu9250_moved 用来保存是否在该定位周期内模块发生运动
if(Count_Anthor() < 4)
{
gProcess_Dis = 0;
BPhero_TAG_Broadcast();
gSend_index = 0;
// HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_8);
isMpu9250_moved = 0;
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
}
else
{
if(gSend_index ==Count_Anthor())
{
gSend_index= 0;
Send_Dis_To_Anthor0();
isMpu9250_moved = 0;
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_7, !GPIO_PIN_SET); //PB7 = 1 LED_ON
} else
{
gProcess_Dis = 1;
BPhero_Distance_Measure_Specail_ANTHOR();// 从1 2 3 4发送
// HAL_GPIO_TogglePin(GPIOB, GPIO_PIN_7);
}
2 标签将isMpu9250_moved 打包并以广播的形式发送给各个模块
static void Send_Dis_To_Anthor0(void)
{
static int framenum = 0 ;
char crc_byte = 0;
//only send this message to anthor0:short address equal 0x0001
msg_f_send.destAddr[0] = 0xFF;
msg_f_send.destAddr[1] = 0xFF;
msg_f_send.seqNum = distance_seqnum;
msg_f_send.messageData[0]='M';
msg_f_send.messageData[1] = 0;//数据包长度
uint8 *pAnthor_Str = &msg_f_send.messageData[2];
int str_len = 0x20;
sprintf(pAnthor_Str, "&&&:%02X$%04X:%d:%02X$",str_len,SHORT_ADDR,isMpu9250_moved,msg_f_send.seqNum);//AA55 ANTHORID
pAnthor_Str = pAnthor_Str + 15+2;
isMpu9250_moved = 0;
for(uint8 index = 0 ; index < MAX_ANTHOR; index++)
{
if(anthor_info[index].alive == 1)
{
sprintf(pAnthor_Str, "%04X:%04X:%02X#",anthor_info[index].short_address,anthor_info[index].distance,anthor_info[index].rssi_info);
pAnthor_Str = pAnthor_Str + 13;
}
}
pAnthor_Str = pAnthor_Str - 1;
sprintf(pAnthor_Str, "$AA##\r\n");
while(msg_f_send.messageData[str_len] != '\n')
{
crc_byte =crc_byte^msg_f_send.messageData[str_len];
str_len++;
}
str_len++;//字符串最后追加'\n'
printf(&msg_f_send.messageData[2]);
msg_f_send.messageData[1] = str_len - 2;//有用数据,其他模块需要传输到串口的数据
HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
dwt_writetxdata(11 + str_len,(uint8 *)&msg_f_send, 0) ; // write the frame data
dwt_writetxfctrl(11 + str_len, 0);
dwt_starttx(DWT_START_TX_IMMEDIATE);
while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
{ };
dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);
framenum++;
LCD_Display_Distance();
}
3 基站收到‘M’信息,通过串口送出
case 'M':
//将收到的距离信息通过串口发送给电脑上位机
USART_puts(&msg_f->messageData[2],msg_f->messageData[1]);
break;
标签发送‘M’信息,第一个字节是‘M’,第二个字节保存了数据长度,在处理‘M’信息,直接使用msg_f->messageData[1] 定义送到串口的数据长度。
上位机测试效果:
源码链接:
固件源码:
固件源码已经放到git上,V1.0 版本开发完成,请详细看下面的描述
https://tuzhuke@bitbucket.org/tuzhuke/bp30_multianthor.git
Hash:3ec3d27cabc914365549fb0d907e034f3caa21ec
测试硬件
4个蓝点BP30 + 1 带黑色外壳的UWB