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

背景：

在前面两个博文中已经提及到，我们打算做一个UWB 结合运动传感器 融合定位，这篇博文实现上位机代码，上位机使用我们之前开源Python版本TWR上位机，代码可以在末尾论坛链接下载

我们的上位机实现基础是之前的《[开源项目] 蓝点无限 UWB Python版本上位机》，参考链接

https://www.cnblogs.com/tuzhuke/p/15170193.html 

再此基础上将《[蓝点无限] UWB 定位数据融合 之 固件实现》

https://www.cnblogs.com/tuzhuke/p/15212574.html 实现完整UWB数据融合项目。

代码全部已经开源，欢迎大家使用并帮忙改进。

直接上代码，代码主要是在《[开源项目] 蓝点无限 UWB Python版本上位机》基础上修改，这里列出代码更改部分。

1 解析数据包中的运动变量，并存放到字典中

    result_dict = {'tag': 0x1005, 'acc':0, 'seq': 7, 'time': 1234, 'anthor_count': 4,'anthor': []}

    # 数据包以&&& 开头
    res = re.findall(r'&&&', string)
    flag = 1
    if len(res) > 0:
        # step1 print message length,ex 76
        temp_string = string.split("$")[0]  # &&&:80$
        data_len = int(temp_string.split(":")[1], 16)

        # tag info
        temp_string = string.split("$")[1]  # 000A:20
        tag_id = int(temp_string.split(":")[0], 16)  # 000A
        tag_acc = int(temp_string.split(":")[1], 16)
        tag_seq = int(temp_string.split(":")[2], 16)  # 20
        # print("标签ID: %02X  Seq: %X" % (tag_id, tag_seq))
        result_dict['tag'] = tag_id
        result_dict['acc'] = tag_acc
        result_dict['seq'] = tag_seq

　

2 在返回结果中，将运动信息一并返回给上层

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

3 顶层收到定位结果和运动信息，打印结果，并发送给处理函数

                [location_result, location_seq, location_addr, location_x, location_y] = twr_main(msg)
                if location_result == 1:
                    self.data_result.emit(
                        '%d %d %0.2f %0.2f' % (location_seq, location_addr, location_x, location_y))
        # #                 bphero_dispose(str(data))

4 UWB和运动信息进行简单融合，当模块静止，不更新坐标信息

   def insert_result(self, input_str):
        strlist = input_str.split(' ')
        location_addr = int(strlist[1])
        location_x = float(strlist[2])
        location_y = float(strlist[3])
        tag_acc = int(strlist[4])
        print("acc = %d"%tag_acc)
        print("insert result")
        if tag_acc == 0:#只有模块移动的时候更新坐标
            self.Insert_Tag_Result(location_addr,
                                   {"x": location_x, "y": location_y, "z": 0, "qt": QGraphicsEllipseItem(-10, -10, 10, 10)})

　其他代码，上位机增加了串口接收功能

class ComThread(QtCore.QThread):
    data_result = QtCore.pyqtSignal(object)
    data_draf = QtCore.pyqtSignal(object)

    def __init__(self):
        super(ComThread, self).__init__()
        self.l_serial = None
        self.alive = False
        self.waitEnd = None
        self.ID = None
        self.data = None
        self.port = None

    def set_port(self,port):
        self.port = port
        print(self.port)

    def waiting(self):
        if not self.waitEnd is None:
            self.waitEnd.wait()

    def SetStopEvent(self):
        if not self.waitEnd is None:
            self.waitEnd.set()
        self.alive = False
        self.stop()

    def start(self):
        self.l_serial = serial.Serial()
        self.l_serial.port = self.port
        self.l_serial.baudrate = 115200
        self.l_serial.timeout = 2
        self.l_serial.open()
        if self.l_serial.isOpen():
            self.waitEnd = threading.Event()
            self.alive = True
            self.thread_read = None
            self.thread_read = threading.Thread(target=self.FirstReader)
            self.thread_read.setDaemon(1)
            self.thread_read.start()
            return True
        else:
            return False

    def SendDate(self, i_msg, send):
        lmsg = ''
        isOK = False
        if isinstance(i_msg):
            lmsg = i_msg.encode('gb18030')
        else:
            lmsg = i_msg
        try:
            # 发送数据到相应的处理组件
            self.l_serial.write(send)
        except Exception as ex:
            pass;
        return isOK

    def FirstReader(self):
        while self.alive:
            data = ''
            data = data.encode('utf-8')
            n = self.l_serial.inWaiting()
            if n:
                data = self.l_serial.readline()
                print(data)
                msg =str(data, encoding="utf-8")
                self.data_draf.emit(msg)  # for debug only

                [location_result, location_seq, location_addr, location_x, location_y, tag_acc] = twr_main(msg)
                print(tag_acc)
                if location_result == 1:
                    self.data_result.emit(
                        '%d %d %0.2f %0.2f %d' % (location_seq, location_addr, location_x, location_y, tag_acc))
        # #                 bphero_dispose(str(data))

        self.waitEnd.set()
        self.alive = False

    def stop(self):
        self.alive = False
        self.thread_read.join()
        if self.l_serial.isOpen():
            self.l_serial.close()

　

以上完成了整个近期打算开源的工程项目，源码请到www.51uwb.cn 下载