[开源项目]蓝点无限TWR算法-多基站多标签固件

初衷：           
       随着UWB大力发展，国内实际应用逐步落地。 对于UWB的需求已经不是停留在实验测试阶段，
       目前逐步进入商用大环境，很多厂商特殊需要一定要选用TWR，无法接收TDOA。
       但是受限于目前大部分TWR方案一般只能支持3-4基站定位，很多厂商无法实现UWB项目快速落地。
       基于以上，我们打算开源一套多基站多标签固件，方便高需求客户见解和二次开发。
       同时，对于学习测试的客户，建议依然使用基本版本3-4基站定位，这种多基站多标签实现增加了更多逻辑部分的实现，不适合入门学习。

软件流程：

关键流程说明：
除了完成基本测距以外，这里主要需要完成动态识别功能，标签需要动态识别它周围的基站，时刻可以保持与周围3-4个基站进行测距。

固件Base：
这个固件依然基于我们的开源框架项目51uwb_base进行二次开发，
更多开源框架相关参见链接：http://51uwb.cn/forum.php?mod=viewthread&tid=165&extra=page%3D1


固件匹配Python开源上位机：
上位机使用我们目前已经开源的纯python版本上位机，相关链接：http://51uwb.cn/forum.php?mod=viewthread&tid=401&extra=page%3D1


固件重要问题说明：
1 串口输出，不同以往，这里用的是标签串口
2 上位机接口目前是TCP，需要使用串口转TCP工具:串口转TCP参见视频https://www.bilibili.com/video/BV1rv411p7Hj/


固件源码：
固件源码已经放到git上，V1.0 版本开发完成，请详细看下面的描述
https://tuzhuke@bitbucket.org/tuzhuke/bp30_multianthor.git

固件开发记录
Day1：
githash:a387f5cdbbff3b6b1c818eaf459b4ad2a6fe24c0

主要完成功能，标签发送广播信号，基站接收广播信号。标签发送的广播信号需要包含已经识别的基站地址。

Step1 在标签中定义一个存放基站的结构体数组

struct Anthor_Information
{
     uint16 short_address;//基站16bit 短地址
     uint16 distance;//距离信息，高8 低8bit
     uint32 last_time;//上次通信时间
     uint8  rssi_info;//上次通信RSSI值记录
    unsigned char alive; //是否已经识别或者是否已经丢失
} anthor_info[MAX_ANTHOR

Step2 在标签中发送广播信号，广播信号包含了已经识别的基站，如果基站收到这个信息，发现数据包中已经有自己地址就无需反馈，否则反馈信息给标签。函数试下如下：

点击查看代码

    /*******************************************************************************
    * 函数名  : BPhero_TAG_Broadcast
    * 描述    : 标签启动发送广播信息给各个基站,信息数据包包括了基站短地址
    * 输入    : 无
    * 输出    : 无
    * 返回值  : 无
    * 说明    : 发送broadcast信息(B信息)给所有基站
    *******************************************************************************/

    void BPhero_TAG_Broadcast(void)
    {
        uint8 index = 0 ;
        uint8 strlen = 0;
        msg_f_send.destAddr[0] = 0xFF;
        msg_f_send.destAddr[1] = 0xFF;

        msg_f_send.seqNum = distance_seqnum;
        msg_f_send.messageData[0]='B';//broadcast message
        strlen = strlen + 1;

        uint8 *pAnthor_Str = &msg_f_send.messageData[1];
        //后面跟基站信息
        for(index = 0 ; index < MAX_ANTHOR; index++)
        {
            if(anthor_info[index].alive == 1)
            {
                sprintf(pAnthor_Str, "%04X:",anthor_info[index].short_address);
                pAnthor_Str = pAnthor_Str + 5;
                strlen = strlen + 5;
            }
        }

        //GPIOB.5设定，兼容之前带PA的模块-->如需求请联系www.51uwb.cn
        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
        //写入数据
        dwt_writetxdata(11 + strlen,(uint8 *)&msg_f_send, 0) ;  // write the frame data
        dwt_writetxfctrl(11 + strlen, 0);
        dwt_starttx(DWT_START_TX_IMMEDIATE);        //启动发送
        while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))        //等待发送完成
        { };
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除发送完成标志
        poll_tx_ts = get_tx_timestamp_u64();//读取发送时间戳

        //清空接收缓存，待收到数据时使用
        for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
        {
            rx_buffer[i] = '';
        }
        dwt_enableframefilter(DWT_FF_DATA_EN);        //启动帧过滤功能
        dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS*10);//设定接收延时函数
        dwt_rxenable(0);//启动接收机
        //sequence控制
        if(++distance_seqnum == 255)
            distance_seqnum = 0;
    }

Step3 在标签中调用上述函数广播发送，我们使用之前代码定时器回调函数，通过回调函数，可以周期性发送

    void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
    {
        if (htim->Instance == htim3.Instance)
        {
            HAL_TIM_Base_Stop(&htim3);
            {
                dwt_forcetrxoff();
                TAG_SendOut_Messge();
                BPhero_TAG_Broadcast();
            }

            HAL_TIM_Base_Start(&htim3);
        }
    }

Step4： 在基站接收broadcast “B”信号，在rx_main.c中收到“B信号”后打印一串字符。

 case 'B':
                printf("receive B message
");
                break;

分别编译标签和基站的进行测试。串口数据如下图，表明数据可以正常收到。

Day2:
githash: e23e4e1e3bf681d1125036e5dbbf5a07fe363fdc

主要完成基站收到B信息后，以R信息回复给标签，标签收到信息提取短地址，并更新自己的结构体数组

Step1 修改标签广播格式，在数据包中增加已知基站的个数。

  //后面跟基站信息
    for(index = 0 ; index < MAX_ANTHOR; index++)
    {
        if(anthor_info[index].alive == 1)
        {
            sprintf(pAnthor_Str, "%04X:",anthor_info[index].short_address);
            pAnthor_Str = pAnthor_Str + 5;
            strlen = strlen + 5;
                                          anthor_count++;
        }
    }
                msg_f_send.messageData[1] = anthor_count;

Step2: 修改基站接收处理，目前只简单反馈信息给标签，以“R”信息回复到标签，同时将标签数据包中的“现有基站个数”打印出来用于debug

    case 'B':
                printf("receive B anthor = %d
",msg_f->messageData[1]);
                {
                    msg_f_send.messageData[0]='R';//Poll message
                    //后面修改这个数据长度
                    dwt_writetxdata(11 + 1, (uint8 *)&msg_f_send, 0) ; // write the frame data
                    dwt_writetxfctrl(11 + 1, 0);
                    dwt_starttx(DWT_START_TX_IMMEDIATE);
                    while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
                    { };
                }
                break;

Step3: 标签收到信息，提取基站信息中的地址

       case 'R':
      address = msg_f_recv->sourceAddr[1]<<8|msg_f_recv->sourceAddr[0];
      printf("receive R message 0x%04X
",address);
      Update_Anthor_Info(address);
      break;

    uint8 Update_Anthor_Info(uint32 shortaddress)
    {
       uint8 index = 0;
       printf("shortaddress = 0x%04X
",shortaddress);
       //后面跟基站信息
        for(index = 0 ; index < MAX_ANTHOR; index++)
        {
            if(anthor_info[index].alive == 0)
            {
                                            anthor_info[index].short_address = shortaddress;
                                            anthor_info[index].alive = 1;        
                                            return 1;
            }
        }
                    return 0;
    }

Day3:
githash:4d1b64584706426c2a71174d7026cbe7696f2a4b

今天完成了基本功能开发，可以作为V1.0版本。
主要开发内容：基站解析标签发送的广播B信号，标签汇总R信号基站，如果收到R基站大于等于4个开始测距，如果测距的时候发现基站丢失，重新启动广播B信号。
1 基站解析标签广播B信号，匹配是否有自己的地址，有地址忽略，没有地址回复R信号

点击查看代码

     case 'B':
                printf("receive B anthor = %d
",msg_f->messageData[1]);
                Num_Anthor = msg_f->messageData[1];
                Sourceaddress =  msg_f->sourceAddr[1]<<8| msg_f->sourceAddr[0];
                pAnthor_Str = &msg_f->messageData[2];
                match_flag = 0;
                for (Index = 0; Index < Num_Anthor; Index++)
                {
                    printf("receive address = %04X
",(pAnthor_Str[1]<<8|pAnthor_Str[0]));

                    if(SHORT_ADDR == (pAnthor_Str[1]<<8|pAnthor_Str[0])) //匹配成功
                    {
                        printf("match
");
                        match_flag = 1;

                    }
                    pAnthor_Str = pAnthor_Str +3 ;
                }

                if(match_flag == 0)//没有匹配到，发送一个反馈信息
                {
                    msg_f_send.messageData[0]='R';//Poll message
                    //后面修改这个数据长度
                    dwt_writetxdata(11 + 1, (uint8 *)&msg_f_send, 0) ; // write the frame data
                    dwt_writetxfctrl(11 + 1, 0);
                    dwt_starttx(DWT_START_TX_IMMEDIATE);
                    while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))
                    { };
                }
                break;

2 标签汇总基站反馈的R信号，其实这部分代码在day2 已经完成，无需修改。
3 判断收到R信号个数，这个在定时广播里判断的。如果小于4，持续执行广播收集。

  HAL_TIM_Base_Stop(&htim3);
        {
            dwt_forcetrxoff();
            if(Count_Anthor() < 4)
            {
                gProcess_Dis = 0;
                BPhero_TAG_Broadcast();
                gSend_index = 0;

            }

具体实现函数

    uint8 Count_Anthor()
    {
        uint8 index = 0;
        uint8 count = 0;
        //后面跟基站信息
        for(index = 0 ; index < MAX_ANTHOR; index++)
        {
            if(anthor_info[index].alive == 1)
            {
                count++;
            }
        }
        return count;
    }

 3 当R信号基站数量等于4个，开始启动测距

点击查看代码

    if(Count_Anthor() < 4)
            {
                gProcess_Dis = 0;
                BPhero_TAG_Broadcast();
                gSend_index = 0;

            }
            else
            {
                if(gSend_index ==Count_Anthor())
                {
                    gSend_index= 0;
                    Send_Dis_To_Anthor0();
                } else
                {
                    gProcess_Dis = 1;
                    BPhero_Distance_Measure_Specail_ANTHOR();// 从1 2 3 4发送
                }
            }

这里的Send_Dis_To_Anthor0()是沿用之前的函数名，其实在这个里面实现了数据格式组装并在串口打印，以及调用函数在液晶显示。
BPhero_Distance_Measure_Specail_ANTHOR()主要功能就是启动测距，测距对象是收集到R信号的基站。

点击查看代码

    void BPhero_Distance_Measure_Specail_ANTHOR(void)
    {
        uint16 destaddress = Find_Address();
       // printf("Send Index = %d, Address = 0x%04X
",gSend_index,destaddress);
        msg_f_send.destAddr[0] =(destaddress) &0xFF;
        msg_f_send.destAddr[1] =  ((destaddress)>>8) &0xFF;

        msg_f_send.seqNum = distance_seqnum;
        msg_f_send.messageData[0]='P';//Poll message

        //GPIOB.5设定，兼容之前带PA的模块-->如需求请联系www.51uwb.cn
        HAL_GPIO_WritePin(GPIOB, GPIO_PIN_5, !GPIO_PIN_RESET);//PA node ,enable pa
        //写入数据
        dwt_writetxdata(12,(uint8 *)&msg_f_send, 0) ;  // write the frame data
        dwt_writetxfctrl(12, 0);
        dwt_starttx(DWT_START_TX_IMMEDIATE);        //启动发送
        while (!(dwt_read32bitreg(SYS_STATUS_ID) & SYS_STATUS_TXFRS))        //等待发送完成
        { };
        dwt_write32bitreg(SYS_STATUS_ID, SYS_STATUS_TXFRS);//清除发送完成标志
        poll_tx_ts = get_tx_timestamp_u64();//读取发送时间戳

        //清空接收缓存，待收到数据时使用
        for (int i = 0 ; i < FRAME_LEN_MAX; i++ )
        {
            rx_buffer[i] = '';
        }
        dwt_enableframefilter(DWT_FF_DATA_EN);        //启动帧过滤功能
        dwt_setrxtimeout(RESP_RX_TIMEOUT_UUS);//设定接收延时函数
        dwt_rxenable(0);//启动接收机
        //sequence控制
        if(++distance_seqnum == 255)
            distance_seqnum = 0;
    }

4 中断回调函数中处理timeout，如果测距对象基站没有反馈，标签发生timeout中断，则立即将该基站islive 设置为0，带下次统计，发现基站数量小于4，则标签重新发送广播信号收集基站

  else
    {
        if(gProcess_Dis == 1)
        {
            printf("timeout address 0x%04X
",Find_Address());
            Delete_Anthor(Find_Address());
        }

其它更新：
git hash:4892896b3d09e6009dfbe3d537e866f2c94d2d36
修改了一个祖传代码bug，使用了野指针....

git hash：d2e01e118126c9ce9c84337126db3e92d23ed3ba
修改了UWB中断，让UWB中断只处理接收成功和timeout两种事件，其他事件均不处理

同时，调试的时候发现上位机当收到异常数据无法处理，导致异常退出

    def twr_main(input_string):
        print(input_string)
        error_flag, result_dic = Process_String_Before_Udp(input_string)
        if error_flag == 0:
            [location_result, location_seq, location_addr, location_x, location_y] = Compute_Location(result_dic)
            return location_result, location_seq, location_addr, location_x, location_y
        return 0, 0, 0, 0, 0

当发生异常，增加return 0, 0, 0, 0, 0，代码同步更新到上位机部分链接。

自此，通过三天，零散的时间，开发出一套可以动态识别基站并完成测距的固件代码，代码编写时间和测试debug时间基本是1:5，更多的是细节考虑步骤导致异常.
开发过程中遇到一个很诡异的异常，当其中一个基站地址将短地址设置为0x0006后，标签识别成功但是无法完成测距，通过加debug信息最终发现是由于标签测距完成后有一个滤波器，滤波器设置的最大基站数目为5，导致数组越界访问。

三天零散的开发时间，可以说这次开发非常顺利，一是由于有一个比较完备的代码框架，基于之前的代码框架开发，可以减少对于底层的依赖，只需要实现逻辑部分即可。而逻辑部分其实在很早之前就有想法，通过想法落实到流程图，规划每一步要做什么。目前的代码还没有切合到流程图上，流程图中，我的想法是即便有4个基站可以定位，依然周期性的发送广播，发现更多的基站，选取附近的基站做参考。 由于时间关系，这个部分可能留给各位看官了。

关于硬件，目前我们的代码，基于硬件是我们自研的BP30，使用主控是F4。同时可以无缝在BP400 上使用。 如果没有我们的硬件，可以适当进行移植，匹配主控。

最后，欢迎交流分享！