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

2. 重新編寫 devRun.py 讓其更好閱讀

src/fc_network_adapter/fc_network_adapter/devRun.py

@@ -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()
 
+    # 做點延遲 讓 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))
+
     start_time = time.time()
     show_time = time.time() 
-    print("connection established!")
+    while time.time() - start_time < running_time:
-
-    print(type(mavlink_socket_out))
-    print(type(mavlink_socket_out.mav))
-
-    while time.time() - start_time < 100:
         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:
+    # 這邊測試 node 生成 topic 的功能
+    # 利用 空的通道 發出假的 heartbeat 封包
+    print('===> Start of Program .Test ', test_item)
+    rclpy.init() # 注意要初始化 rclpy 才能使用 node
+    
+    from pymavlink.dialects.v20 import common as mavlink2
+
     sysid = 1
     compid = 1
 
-    # 這邊測試 node 生成 topic 的功能
-    rclpy.init() # 注意要初始化 rclpy 才能使用 node
-    # mavlink_object_none = mo.mavlink_object(None)
-
     # 啟動 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()

src/fc_network_adapter/fc_network_adapter/mavlinkDevice.py

@@ -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):

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -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:

src/unitdev02/unitdev02/test01.py

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