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