[藍(lán)點(diǎn)無(wú)限] UWB 定位數(shù)據(jù)融合 之 上位機(jī)實(shí)現(xiàn)
背景:
在前面兩個(gè)博文中已經(jīng)提及到,我們打算做一個(gè)UWB 結(jié)合運(yùn)動(dòng)傳感器 融合定位,這篇博文實(shí)現(xiàn)上位機(jī)代碼,上位機(jī)使用我們之前開(kāi)源Python版本TWR上位機(jī),代碼可以在末尾論壇鏈接下載
我們的上位機(jī)實(shí)現(xiàn)基礎(chǔ)是之前的《[開(kāi)源項(xiàng)目] 藍(lán)點(diǎn)無(wú)限 UWB Python版本上位機(jī)》,參考鏈接
http://www.rzrgm.cn/tuzhuke/p/15170193.html
再此基礎(chǔ)上將《[藍(lán)點(diǎn)無(wú)限] UWB 定位數(shù)據(jù)融合 之 固件實(shí)現(xiàn)》
http://www.rzrgm.cn/tuzhuke/p/15212574.html 實(shí)現(xiàn)完整UWB數(shù)據(jù)融合項(xiàng)目。
代碼全部已經(jīng)開(kāi)源,歡迎大家使用并幫忙改進(jìn)。
直接上代碼,代碼主要是在《[開(kāi)源項(xiàng)目] 藍(lán)點(diǎn)無(wú)限 UWB Python版本上位機(jī)》基礎(chǔ)上修改,這里列出代碼更改部分。
1 解析數(shù)據(jù)包中的運(yùn)動(dòng)變量,并存放到字典中
result_dict = {'tag': 0x1005, 'acc':0, 'seq': 7, 'time': 1234, 'anthor_count': 4,'anthor': []}
# 數(shù)據(jù)包以&&& 開(kāi)頭
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("標(biāo)簽ID: %02X Seq: %X" % (tag_id, tag_seq))
result_dict['tag'] = tag_id
result_dict['acc'] = tag_acc
result_dict['seq'] = tag_seq
2 在返回結(jié)果中,將運(yùn)動(dòng)信息一并返回給上層
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 頂層收到定位結(jié)果和運(yùn)動(dòng)信息,打印結(jié)果,并發(fā)送給處理函數(shù)
[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和運(yùn)動(dòng)信息進(jìn)行簡(jiǎn)單融合,當(dāng)模塊靜止,不更新坐標(biāo)信息
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:#只有模塊移動(dòng)的時(shí)候更新坐標(biāo)
self.Insert_Tag_Result(location_addr,
{"x": location_x, "y": location_y, "z": 0, "qt": QGraphicsEllipseItem(-10, -10, 10, 10)})
其他代碼,上位機(jī)增加了串口接收功能
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:
# 發(fā)送數(shù)據(jù)到相應(yīng)的處理組件
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()
以上完成了整個(gè)近期打算開(kāi)源的工程項(xiàng)目,源碼請(qǐng)到www.51uwb.cn 下載

浙公網(wǎng)安備 33010602011771號(hào)