chiyu1468
/
AirTrapMine
2
0
Fork 1
Code Issues Pull Requests Packages Projects Releases Wiki Activity

1. 修改 mavlinkObject.py 無法順利識別多台 vehicle 的重大bug

Browse Source 
2. 重新編寫 devRun.py 讓其更好閱讀
chiyu
Chiyu Chen 12 months ago
parent 84119b788e
commit f9080a5015
4 changed files with 116 additions and 86 deletions
Show Stats Download Patch File Download Diff File

151
src/fc_network_adapter/fc_network_adapter/devRun.py
Unescape Escape View File

@ -10,14 +10,16 @@ from pymavlink import mavutil
# 自定義的 import
import mavlinkObject as mo
import mavlinkDevice as md
# ====================== 分割線 =====================
test_item = 22
running_time = 20
print('test_item : ', test_item)
if test_item == 1:
# 測試 updateMultiplexingList 的輸出
# 測試 mavlink_object 中 updateMultiplexingList 的輸出
print('===> Start of Program .Test ', test_item)
mavlink_object_none = mo.mavlink_object(None)
@ -55,9 +57,9 @@ if test_item == 1:
print('End of Program')
elif test_item == 2:
# 測試 updateMultiplexingList 的輸出
# 測試 mavlink_object 創建時 socket_id 是否正確
# 說實在 這個測試項 似乎因為 python 的 GC 機制 會導致難以測試
print('===> Start of Program .Test ', test_item)
mavlink_object_none1 = mo.mavlink_object(None)
mavlink_object_none2 = mo.mavlink_object(None)
@ -73,33 +75,36 @@ elif test_item == 2:
print('End of Program')
elif test_item == 10:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
# 只啟用了 mavlink_object 的功能
print('===> Start of Program .Test ', test_item)
# 創建一個空的通道 這個通道的 socket_id 是 0
mavlink_object_none = mo.mavlink_object(None)
# 創建另一個通道
connection_string="udp:127.0.0.1:14550"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object1 = mo.mavlink_object(mavlink_socket)
print(mavlink_object1.multiplexingToSwap)
print(mo.swap_queues)
# 設定通道轉發的參數
mavlink_object1.multiplexingToAnalysis = [30] # only ATTITUDE
mavlink_object1.multiplexingToReturn = [0] # only HEARTBEAT
mavlink_object1.multiplexingToSwap[0] = [74, ] # only VFR_HUD
# mavlink_object1.multiplexingToSwap[0] = [-1, ] # all
# 啟動連線的模組
# print(mavlink_object1.multiplexingToSwap)
# print(mo.swap_queues)
# 啟動通道
mavlink_object1.run()
print(mavlink_object1._multiplexingList)
# 確認轉拋的設定有沒有錯
print("_multiplexingList for mavlink object :", mavlink_object1._multiplexingList)
# 運行幾秒並印出 queue 的資料
start_time = time.time()
while time.time() - start_time < 3:
while time.time() - start_time < running_time:
while not mo.fixed_stream_bridge_queue.empty():
print('fixed_stream_bridge_queue:')
t = mo.fixed_stream_bridge_queue.get()
@ -125,28 +130,38 @@ elif test_item == 10:
mavlink_socket.close()
print('End of Program')
elif test_item == 11:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 確認 analyzer 運行後對於 device object 的建立與封包統計狀況
# 啟用 mavlink_object 與 mavlink_bridge的thread區塊 的功能
print('===> Start of Program .Test ', test_item)
connection_string="udp:127.0.0.1:14550"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object2 = mo.mavlink_object(mavlink_socket)
mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT
mavlink_object2.multiplexingToReturn = []
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
# 創建通道
connection_string="udp:127.0.0.1:14550"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object2 = mo.mavlink_object(mavlink_socket)
# mavlink_object2.multiplexingToAnalysis = [0] # only HEARTBEAT
# mavlink_object2.multiplexingToReturn = []
# 啟動連線的模組
mavlink_object2.run()
# 做點延遲 讓 heartbeat 先吃進來
time.sleep(2)
print("=== connection established! ===")
# 印出目前所有 mavlink_systems 的內容
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
start_time = time.time()
show_time = time.time()
compid = 1
while time.time() - start_time < 5:
while time.time() - start_time < running_time:
if (time.time() - show_time) >= 1:
show_time = time.time()
for sysid in analyzer.mavlink_systems:
@ -156,14 +171,6 @@ elif test_item == 11:
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
print("===================")
# 印出目前所有 mavlink_systems 的內容
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
# 結束程式 退出所有 thread
mavlink_object2.stop()
mavlink_object2.thread.join()
@ -175,46 +182,51 @@ elif test_item == 11:
print('End of Program')
elif test_item == 12:
# 我這邊會測試 mavlink object 作為交換器的功能
# 需要開啟一個 ardupilot 的模擬器 與 GCS
# 這邊是測試 mavlink object 作為交換器功能的代碼
print('===> Start of Program .Test ', test_item)
# 初始化兩個通道
# 啟動連線的模組
analyzer = mo.mavlink_bridge()
# 初始化輸入通道
connection_string_in="udp:127.0.0.1:15551"
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
mavlink_object_in.multiplexingToAnalysis = [0] # only HEARTBEAT
# 初始化輸出通道
connection_string_out="udpout:127.0.0.1:14553"
mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200)
mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
mavlink_object_out.multiplexingToAnalysis = [0]
# 做一個空的通道驗證 可以拿來 debug
mavlink_object_none = mo.mavlink_object(None)
# 讓兩個通道互相傳輸
mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
# 做一個空的通道驗證
mavlink_object_none = mo.mavlink_object(None)
# mavlink_object_out.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ] # all
# 啟動連線的模組
analyzer = mo.mavlink_bridge()
# 啟動通道
mavlink_object_in.run()
mavlink_object_out.run()
start_time = time.time()
show_time = time.time()
print("connection established!")
# 做點延遲 讓 heartbeat 先吃進來
time.sleep(3)
print("=== connection established! ===")
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
print(type(mavlink_socket_out))
print(type(mavlink_socket_out.mav))
# print(type(mavlink_socket_out))
# print(type(mavlink_socket_out.mav))
while time.time() - start_time < 100:
start_time = time.time()
show_time = time.time()
while time.time() - start_time < running_time:
try:
test = mo.swap_queues[mavlink_object_none.socket_id].get(block=False)
print('none object : ', test)
@ -226,14 +238,9 @@ elif test_item == 12:
for sysid in analyzer.mavlink_systems:
for compid in analyzer.mavlink_systems[sysid].components:
print("Sysid : {} ,目前收到的訊息數量 : {}".format(sysid, analyzer.mavlink_systems[sysid].components[compid].msg_count))
analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
print("===================")
print('目前所有的系統 : ')
for sysid in analyzer.mavlink_systems:
print(analyzer.mavlink_systems[sysid])
# 結束程式 退出所有 thread
@ -248,18 +255,19 @@ elif test_item == 12:
print('End of Program')
elif test_item == 20:
sysid = 1
compid = 1
# 這邊測試 node 生成 topic 的功能
# 利用 空的通道 發出假的 heartbeat 封包
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
# mavlink_object_none = mo.mavlink_object(None)
from pymavlink.dialects.v20 import common as mavlink2
sysid = 1
compid = 1
# 啟動 mavlink_bridge
analyzer = mo.mavlink_bridge()
from pymavlink.dialects.v20 import common as mavlink2
mav = mavlink2.MAVLink(None)
mav.srcSystem = sysid
mav.srcComponent = compid
@ -282,12 +290,15 @@ elif test_item == 20:
time.sleep(0.1)
print(analyzer.mavlink_systems)
# ROS2 初始化
analyzer._init_node()
# 創建 ROS2 topic
analyzer.create_flightMode(sysid, analyzer.mavlink_systems[sysid].components[compid]) # sysid, compid
print("topic created")
time.sleep(5)
# 丟出 topic
analyzer.emit_info()
# 結束程式
@ -299,9 +310,10 @@ elif test_item == 20:
analyzer.thread.join()
print('End of Program')
elif test_item == 21:
# 需要開啟一個 ardupilot 的模擬器
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
@ -316,11 +328,11 @@ elif test_item == 21:
connection_string="udp:127.0.0.1:15551"
mavlink_socket = mavutil.mavlink_connection(connection_string)
mavlink_object3 = mo.mavlink_object(mavlink_socket)
# 啟動通道
mavlink_object3.run()
print('waiting for mavlink data ...')
print('=== waiting for mavlink data ...')
time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
print('目前所有的系統 : ')
@ -338,7 +350,7 @@ elif test_item == 21:
start_time = time.time()
show_time = time.time()
while time.time() - start_time < 10:
while time.time() - start_time < running_time:
try:
# rclpy.spin(analyzer)
analyzer.emit_info() # 這邊是測試 node 的運行
@ -346,9 +358,10 @@ elif test_item == 21:
except KeyboardInterrupt:
break
# 程式結束
analyzer.destroy_node()
rclpy.shutdown()
rclpy.shutdown()
# 結束程式 退出所有 thread
mavlink_object3.stop()
@ -359,8 +372,8 @@ elif test_item == 21:
mavlink_socket.close()
print('End of Program')
elif test_item == 22:
# 需要開啟一個 ardupilot 的模擬器 與 GCS
# 這邊是測試代碼 引入 rclpy 來測試 node 的運行
print('===> Start of Program .Test ', test_item)
rclpy.init() # 注意要初始化 rclpy 才能使用 node
@ -377,16 +390,17 @@ elif test_item == 22:
mavlink_socket_in = mavutil.mavlink_connection(connection_string_in)
mavlink_object_in = mo.mavlink_object(mavlink_socket_in)
connection_string_out="udpout:127.0.0.1:14553"
mavlink_socket_out = mavutil.mavlink_connection(connection_string_out)
mavlink_object_out = mo.mavlink_object(mavlink_socket_out)
mavlink_object_out.multiplexingToAnalysis = []
mavlink_object_none = mo.mavlink_object(None)
# 讓兩個通道互相傳輸
mavlink_object_in.multiplexingToSwap[mavlink_object_out.socket_id] = [-1, ] # all
mavlink_object_out.multiplexingToSwap[mavlink_object_in.socket_id] = [-1, ] # all
mavlink_object_in.multiplexingToSwap[mavlink_object_none.socket_id] = [-1, ]
# 啟動通道
mavlink_object_in.run()
@ -411,13 +425,14 @@ elif test_item == 22:
show_time = time.time()
show_time2 = time.time()
while time.time() - start_time < 100:
while time.time() - start_time < running_time:
if (time.time() - show_time2) >= 1:
analyzer.emit_info() # 這邊是測試 node 的運行
# sss = analyzer.mavlink_systems[1].components[1].emitParams['flightMode_mode']
fmsg = analyzer.mavlink_systems[1].components[1].emitParams['flightMode']
sss = mavutil.mode_string_v10(fmsg)
print("目前的飛行模式 : ", sss)
# print("目前的飛行模式 : {}, Msg Seq : {}".format(sss, fmsg.get_seq()))
print("目前的飛行模式 : {}".format(sss))
show_time2 = time.time()
# if (time.time() - show_time) >= 2:
@ -428,7 +443,7 @@ elif test_item == 22:
# analyzer.mavlink_systems[sysid].resetComponentPacketCount(compid)
# print("===================")
time.sleep(1)
analyzer.destroy_node()
rclpy.shutdown()

5
src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py
Unescape Escape View File

@ -20,14 +20,15 @@ class mavlink_device():
self.components = {} # 用來記錄每個 component 的資訊 key 是 compid, value 是 component object
def __str__(self):
p_str = ''
p_str = '=====================\n'
p_str += f'object id : {id(self)}\n' # debug
p_str += f'socket_id : {self.socket_id}\n'
p_str += f'sysid : {self.sysid}\n'
p_str += 'has components : \n'
for compid in self.components:
p_str += f'compid : {compid}\n'
p_str += f'mav_type : {self.components[compid].mav_type}\n'
p_str += '=====================\n'
# p_str += '=====================\n'
return p_str
def updateComponentPacketCount(self, compid, current_seq, current_type, current_time):

43
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
Unescape Escape View File

@ -55,7 +55,6 @@ class mavlink_bridge(Node, mavlink_publisher):
藉由控制 ros2 的機制再把 device object 的資訊發送出去
ps. 我限制了這個 class 只能有一個 instance
'''
_instance = None
_lock = threading.Lock() # 確保多線程安全
@ -96,28 +95,41 @@ class mavlink_bridge(Node, mavlink_publisher):
msg = msg_pack[2]
sysid = msg.get_srcSystem()
compid = msg.get_srcComponent()
# 若這個 system id 還不存在 則建立 device object
if not sysid in self.mavlink_systems:
this_device = mavlink_device() # 創建一個新的 device object
self.mavlink_systems[sysid] = this_device
this_device.socket_id = msg_pack[0]
this_device.sysid = sysid
else:
this_device = self.mavlink_systems[sysid]
# 若該 component id 存在 則直接使用該 component object
# 若該 component id 不存在 則利用 heartbeat 創建一個新的 component object
# 若該 component id 不存在 又不是 heartbeat 則不處理
if compid in self.mavlink_systems[sysid].components:
this_component = self.mavlink_systems[sysid].components[compid]
elif msg.get_msgId() == 0:
# 只有透過 heartbeat 可以創建一個新的 component object
this_component = this_device.mavlink_component()
this_device.components[msg.get_srcComponent()] = this_component
this_component.mav_type = msg.type
this_component.mav_autopilot = msg.autopilot
else:
continue
# ↓↓↓↓↓↓↓↓↓↓↓↓ 處理不同訊息類型的功能寫在這裡 請加在這個 elif 之內 ↓↓↓↓↓↓↓↓↓↓↓↓
if msg.get_msgId() == 0: # HEARTBEAT 處理
# 若這個 system id 還不存在 執行完整建立 device object 的流程
if not sysid in self.mavlink_systems:
device_object = mavlink_device() # 創建一個新的 device object
self.mavlink_systems[sysid] = device_object
device_object.socket_id = msg_pack[0]
device_object.sysid = sysid
this_component = device_object.mavlink_component() # 創建一個新的 component object
device_object.components[msg.get_srcComponent()] = this_component
this_component.mav_type = msg.type
this_component.mav_autopilot = msg.autopilot
this_component.emitParams['base_mode'] = msg.base_mode
this_component.emitParams['flightMode_mode'] = mavutil.mode_string_v10(msg)
this_component.emitParams['flightMode'] = msg # debug
# print("mav_type : ", msg.type) # debug
# print("get mode :", mavutil.mode_string_v10(msg)) # debug
# print("record mode :", this_component.emitParams['flightMode_mode']) # debug
this_component.emitParams['flightMode'] = msg # debug
elif msg.get_msgId() == 147: # BATTERY_STATUS 處理
this_component = self.mavlink_systems[sysid].components[msg.get_srcComponent()]
@ -303,7 +315,6 @@ class mavlink_object():
# 處理要送出的封包
# 如果 有資料在 output_buffer 中則將其取出並發送
# 沒有就略過發送
try:
mavlinkMsg_send = self.output_buffer.get(block=False)
except queue.Empty:

3
src/unitdev02/unitdev02/test01.py
Unescape Escape View File

@ -257,3 +257,6 @@ print('Start')
simple_getMavlink()
# mavlink_socket_out = mavutil.mavlink_connection(connection_string_out, source_system=17, source_component=200)

Write Preview
Loading…
Cancel
Save