背景:
在前面两个博文中已经提及到,我们打算做一个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 下载