Enhance mavlinkObject with multiplexing capabilities and add error handling; update README with team member assignments

README.md

@@ -17,6 +17,10 @@
 === 
 Package 簡述
 1. unitdev 為各自協作者做開發時的測試區
+    01 -> 晉凱(ken)
+    02 -> 其宇(chiyu)
+    03 -> 文鈞
+    04 -> 倫渝
 2. fc_network_adapter 
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -1,5 +1,6 @@
 import threading
 from pymavlink import mavutil
+import queue
 
 import rclpy
 from rclpy.node import Node
@@ -7,6 +8,7 @@ from std_msgs.msg import String
 
 import time
 
+
 '''
 
 # 不同的匯流排只有單一種通訊協定 
@@ -19,24 +21,45 @@ import time
 # pymavlink 的 socket 會由其他地方製作(例如 main) 再放到 mavlinkObject 裡面
 # 這邊的 main 會是用來初始化 mavlinkObject 並啟動他的 run function
 
+
 '''
 
+fixed_stream_analyzer_queue = queue.Queue()
+return_packet_processor_queue = queue.Queue()
+converte_format_queues = []
+
 class MavlinkObject():
-    def __init__(self, mavlink_socket):
+    def __init__(self, mavlink_socket, socket_id = None):
+        self.socket_id = socket_id
         self.mavlink_socket = mavlink_socket
         self.msg_count = {}
-        # self.multiplexingDict = {}
-        self.multiplexingToAnalysis = []
-        self.multiplexingToReturn = []
-        self.multiplexingToConvert = []
+        self.multiplexingList = []
+
+        self.multiplexingToAnalysis = [
+            # 0, # HEARTBEAT
+            30, # ATTITUDE
+            33, # GLOBAL_POSITION_INT
+            # 147, # BATTERY_STATUS
+        ]
+        self.multiplexingToReturn = [
+            0, # HEARTBEAT
+            # 30, # ATTITUDE
+        ]
+        self.multiplexingToConvert = [
+            [74, ] # VFR_HUD 
+        ]
     def __del__(self):
         self.mavlink_socket.close()
         self.stop()
 
     def run(self):
+        if not self.socket_id:
+            print('[mavlinkObject.py] Please set socket id before running')
+            return
         self.thread = threading.Thread(target=self._run_thread)
-        self.running = True
+        self.running = self.updateMultiplexingList()
         self.thread.start()
+
     def stop(self):
         self.running = False
 
@@ -45,25 +68,39 @@ class MavlinkObject():
         while self.running:
             try:
                 mavlinkMsg = self.mavlink_socket.recv_msg()
-                if mavlinkMsg:
-                    self.count_mavlink_type(mavlinkMsg)
-                    # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # debug
-                    # break # Debug
-                    
             except Exception as e:
-                print(f"Error receiving mavlink data: {e}")
+                print(f"[mavlinkObject.py] Error receiving mavlink data: {e}")
                 print(mavlinkMsg)
                 self.running = False
                 break
 
+            if mavlinkMsg:
+                self.count_mavlink_type(mavlinkMsg) # 統計收到的訊息
+                # print(mavlinkMsg) # data type 'pymavlink.dialects.v20.ardupilotmega.MAVLink_attitude_message' # 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,mavlinkMsg))
+                        elif i == 1:
+                            return_packet_processor_queue.put((self.socket_id,mavlinkMsg))
+                        else:
+                            convert_queue = converte_format_queues[i-2]
+                            convert_queue.put((self.socket_id,mavlinkMsg))
+
             # 每秒中 統計一次 收到的訊息總量
             elapsed_time = time.time() - start_time
             if elapsed_time > 1:
-                print('Messages Type received (in about 1 sec) :')
+                print('[mavlinkObject.py] Messages Type received (in about 1 sec) :')
                 print(self.msg_count)
                 start_time = time.time()
                 self.msg_count = {}
         
+        # thread 結束
+        print('[mavlinkObject.py] End of _run_thread')
+
     def count_mavlink_type(self, mavlinkMsg):
         msg_type = mavlinkMsg.get_type()
         if msg_type in self.msg_count:
@@ -76,6 +113,45 @@ class MavlinkObject():
         msg.data = str(mavlinkMsg)
         self.publisher_.publish(msg)
 
+    def updateMultiplexingList(self):
+        # 更新 multiplexing list 
+        self.multiplexingList = [self.multiplexingToAnalysis, self.multiplexingToReturn] + self.multiplexingToConvert
+
+        check = all(isinstance(i, list) and all(isinstance(j, int) for j in i) for i in self.multiplexingList)
+
+        # 若有錯誤則回傳 False
+        if not check:
+            print('[mavlinkObject.py] Multiplexing List Error, Please check the list')
+            return False
+        return True
+
+class mavlink_analyzer(Node):
+    '''
+    這個 class 是用來接收 mavlink 訊息 並進行分析
+    記錄有 mavlink bus 上有那些 system id 和 component id
+    為了每個 system id 都有一個對應的 device object
+    並且看是否有重複 system id
+
+    也許可以用 node 去創建 這樣就可以直接進到 ros2 執行緒?
+    '''
+    def __init__(self):
+        super().__init__('mavlink_analyzer')
+
+        # self.publisher_ = self.create_publisher(String, 'mavlink_data', 10)
+        # self.mavlink_object = None
+        # self.create_mavlink_object()
+
+    def create_mavlink_object(self):
+        # connection_string="udp:
+        pass
+
+class mavlink_device():
+    def __init__(self):
+        self.socket_id = None # 記錄來自於哪個 socket
+        self.sysid = None
+        self.component_count = 1
+        self.compid_list = []
+        
 
 
 def main(args=None):
@@ -83,10 +159,34 @@ def main(args=None):
     connection_string="udp:127.0.0.1:14550"
     mavlink_socket = mavutil.mavlink_connection(connection_string)
 
-    mavlink_object = MavlinkObject(mavlink_socket)
+    # 這邊是測試代碼 運行10秒 過程中把三個 queue 的資料印出來
+    mavlink_object = MavlinkObject(mavlink_socket, 1)
+    mavlink_object.multiplexingToAnalysis = [30] # only ATTITUDE
+    mavlink_object.multiplexingToReturn = [0] # only HEARTBEAT
+    mavlink_object.multiplexingToConvert = [[74,]] # only VFR_HUD 
+    
+    converte_format_queues.append(queue.Queue())
+
     mavlink_object.run()
+    start_time = time.time()
+    while time.time() - start_time < 2:
+        
+        while not fixed_stream_analyzer_queue.empty():
+            print('fixed_stream_analyzer_queue:')
+            t = fixed_stream_analyzer_queue.get()
+            print('from {} : {}'.format(t[0],t[1]))
+        while not return_packet_processor_queue.empty():
+            print('return_packet_processor_queue:')
+            t = return_packet_processor_queue.get()
+            print('from {} : {}'.format(t[0],t[1]))
+        
+        for q in converte_format_queues:
+            while not q.empty():
+                print('converte_format_queues:')
+                t = q.get()
+                print('from {} : {}'.format(t[0],t[1]))
+
 
-    time.sleep(5)
     mavlink_object.running = False
     mavlink_object.thread.join()
     print('End of Program')

src/unitdev02/unitdev02/test01.py

@@ -62,7 +62,6 @@ def fdm_parser_example(data=None):
         data = bytes.fromhex('1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000')
     # 1a48e903b1340500e803e803e803e803000000000000000000000000000000000000000000000000
 
-
     parse_format = 'HHI16H'
 
     if len(data) != struct.calcsize(parse_format):
@@ -226,4 +225,6 @@ print('Start')
 # fdm_switch_example_two()
 # fdm_parser_example()
 # mavlink_analyzer_simple(8)
-mavlink_btye_generator_test()
+# mavlink_btye_generator_test()
+
+