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

code 沒有動 只是調整版面 與 新增一些註解 增加可讀性

Browse Source
chiyu
Chiyu Chen 1 year ago
parent bdd6cceb96
commit 5c08a961cf
1 changed files with 125 additions and 103 deletions
Show Stats Download Patch File Download Diff File

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

@ -30,6 +30,7 @@ from theLogger import setup_logger
# ====================== 分割線 =====================
logger = setup_logger("mavlinkObject.py")
fixed_stream_analyzer_queue = queue.Queue()
@ -40,107 +41,7 @@ converte_format_queues = []
# 資料格式 { sysid : mavlink_device object }
mavlink_systems = {}
class mavlink_object():
def __init__(self, mavlink_socket, socket_id = None):
self.socket_id = socket_id
self.mavlink_socket = mavlink_socket
self.msg_count = {}
self.multiplexingList = []
# 關聯到全域變數
global mavlink_systems
self.mavlink_systems = mavlink_systems
# 這三個 list 用來分配不同的訊息到不同的 queue
self.multiplexingToAnalysis = [
0, # HEARTBEAT
24, # GPS_RAW_INT
30, # ATTITUDE
33, # GLOBAL_POSITION_INT
74, # VFR_HUD
147, # BATTERY_STATUS
]
self.multiplexingToReturn = [
0, # HEARTBEAT
# 30, # ATTITUDE
]
self.multiplexingToConvert = [
]
def __del__(self):
self.mavlink_socket.close()
self.stop()
def run(self):
if not self.socket_id:
logger.error('Please set socket id before running')
return
self.thread = threading.Thread(target=self._run_thread)
self.running = self.updateMultiplexingList()
self.thread.start()
def stop(self):
self.running = False
def _run_thread(self):
logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
start_time = time.time()
while self.running:
timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差
try:
mavlinkMsg = self.mavlink_socket.recv_msg()
except Exception as e:
logger.critical(f"Receiving data not mavlink format. Object Delete.")
print(f"[mavlinkObject.py] Error receiving mavlink data: {e}")
print(mavlinkMsg)
self.running = False
break
if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message'
# 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間
sysid = mavlinkMsg.get_srcSystem()
compid = mavlinkMsg.get_srcComponent()
if sysid in self.mavlink_systems: #　只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息
# mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型
mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp)
# print(mavlinkMsg) # debug
# break # Debug
# 將訊息依照 multiplexing list 分發到不同的 queue
for i in range(len(self.multiplexingList)):
if mavlinkMsg.get_msgId() in self.multiplexingList[i]:
if i == 0:
fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg))
elif i == 1:
return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg))
else:
convert_queue = converte_format_queues[i-2]
convert_queue.put((self.socket_id,timestamp,mavlinkMsg))
# thread 結束
logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id))
def updateMultiplexingList(self):
'''
更新 multiplexing list 並做簡單的檢查
'''
# 更新 multiplexing list
self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert
# 檢查 multiplexing list 格式是否有錯誤
check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList)
if not check:
logger.error('Multiplexing List Format Error, Please check the list')
return False # 若有錯誤則回傳 False
check = len(self.multiplexingToConvert) != len(converte_format_queues)
if check:
logger.error('Multiplexing Queue not fit List , Please check the list')
return False
return True
# ===================== 分割線 =====================
# ====================== 分割線 =====================
class mavlink_analyzer(Node, mavlink_publisher):
'''
@ -170,6 +71,7 @@ class mavlink_analyzer(Node, mavlink_publisher):
if cls._instance is None:
cls._instance = super(mavlink_analyzer, cls).__new__(cls)
return cls._instance
def __init__(self):
if not hasattr(self, "initialized"): # 防止重複初始化
self.initialized = True
@ -184,6 +86,7 @@ class mavlink_analyzer(Node, mavlink_publisher):
self.thread.start()
else:
logger.error('mavlink_analyzer instance already exists. Do not create another one.')
def stop(self):
self.running = False
@ -253,8 +156,127 @@ class mavlink_analyzer(Node, mavlink_publisher):
# 再註銷 node
pass
# ====================== 分割線 =====================
def main(args=None):
class mavlink_object():
'''
每個 mavlink bus 都會有一個 mavlink_object
其中主要是 thread 做統計封包與分流
分流表的控制在三個 list 分別為
multiplexingToAnalysis : 這個 list 是用來分流到固定串流分析器的
multiplexingToReturn : 這個 list 是用來分流到回傳封包處理器的
multiplexingToConvert : 這個 list 是用來分流到轉格式的
透過先更新上述三個 list 後再執行 updateMultiplexingList 可以變更分流行為
'''
def __init__(self, mavlink_socket, socket_id = None):
self.socket_id = socket_id
self.mavlink_socket = mavlink_socket
self.msg_count = {}
self.multiplexingList = []
# 關聯到全域變數
global mavlink_systems
self.mavlink_systems = mavlink_systems
# 這三個 list 用來分配不同的訊息到不同的 queue
self.multiplexingToAnalysis = [
0, # HEARTBEAT
24, # GPS_RAW_INT
30, # ATTITUDE
33, # GLOBAL_POSITION_INT
74, # VFR_HUD
147, # BATTERY_STATUS
]
self.multiplexingToReturn = [
0, # HEARTBEAT
# 30, # ATTITUDE
]
self.multiplexingToConvert = [
]
def __del__(self):
self.mavlink_socket.close()
self.stop()
def run(self):
if not self.socket_id:
logger.error('Please set socket id before running')
return
self.thread = threading.Thread(target=self._run_thread)
self.running = self.updateMultiplexingList()
self.thread.start()
def stop(self):
self.running = False
def _run_thread(self):
logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
start_time = time.time()
while self.running:
timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差
try:
mavlinkMsg = self.mavlink_socket.recv_msg()
except Exception as e:
logger.critical(f"Receiving data not mavlink format. Object Delete.")
print(f"[mavlinkObject.py] Error receiving mavlink data: {e}")
print(mavlinkMsg)
self.running = False
break
if mavlinkMsg: # data type should be 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message'
# 統計收到的訊息 & 透過 mavlink 封包的序列號來檢查是否有遺失的封包 & 記錄最後收到的封包時間
sysid = mavlinkMsg.get_srcSystem()
compid = mavlinkMsg.get_srcComponent()
if sysid in self.mavlink_systems: #　只有當這個 system id 已經透過 HEARTBEAT 訊號被初始化過 才會記錄相關訊息
# mavlinkMsg.get_type() # mavlinkMsg.get_msgId() # 前者是字串 後者是 int 都是代表 mavlink 類型
mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp)
# print(mavlinkMsg) # debug
# break # Debug
# 將訊息依照 multiplexing list 分發到不同的 queue
for i in range(len(self.multiplexingList)):
if mavlinkMsg.get_msgId() in self.multiplexingList[i]:
if i == 0:
fixed_stream_analyzer_queue.put((self.socket_id,timestamp,mavlinkMsg))
elif i == 1:
return_packet_processor_queue.put((self.socket_id,timestamp,mavlinkMsg))
else:
convert_queue = converte_format_queues[i-2]
convert_queue.put((self.socket_id,timestamp,mavlinkMsg))
# thread 結束
logger.info('End of mavlink_object._run_thread id : {}'.format(self.socket_id))
def updateMultiplexingList(self):
'''
更新 multiplexing list 並做簡單的檢查
'''
# 更新 multiplexing list
self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert
# 檢查 multiplexing list 格式是否有錯誤
check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList)
if not check:
logger.error('Multiplexing List Format Error, Please check the list')
return False # 若有錯誤則回傳 False
check = len(self.multiplexingToConvert) != len(converte_format_queues)
if check:
logger.error('Multiplexing Queue not fit List , Please check the list')
return False
return True
# ====================== 分割線 =====================
# 整合到 ros2 之後的程式進入點
def main_node():
pass
# ====================== 分割線 =====================
# 測試時的程式進入點
def main_develop(args=None):
test_item = 2
print('test_item : ', test_item)
@ -411,4 +433,4 @@ def main(args=None):
if __name__ == '__main__':
import rclpy # 這邊是為了測試而加的
main()
main_develop()

Write Preview
Loading…
Cancel
Save