[蓝点无限] UWB 定位数据融合 之 固件实现

 在前面两个博文中已经提及到，我们打算做一个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