Upload files to 'src/fc_network_adapter/fc_network_adapter'

2025-06-30 19:43:36 +08:00
parent 68af81ac68
commit a55b76534f
1 changed files with 36 additions and 58 deletions
@@ -464,69 +464,47 @@ elif test_item == 22:
    print('<=== End of Program')

 elif test_item == 51:
+    print("===> Start of Program .Test", test_item)
+    rclpy.init()

-    # 晉凱的測試項目
-    print('===> Start of Program .Test ', test_item)
-    rclpy.init() # 注意要初始化 rclpy 才能使用 node
+    # 1) 啟動 bridge（它已自動建立所有 publisher）
+    bridge = mo.mavlink_bridge()
+    bridge._init_node()

-    # 啟動 mavlink_bridge
-    analyzer = mo.mavlink_bridge()
-    # 關於 Node 的初始化
-    show_time = time.time()
-    analyzer._init_node() # 初始化 node
-    print('初始化 node 完成 耗時 : ',time.time() - show_time)
+    # 2) 建立一條 UDP 通道並啟動 mavlink_object
+    mav_sock = mavutil.mavlink_connection("udp:127.0.0.1:14550")
+    mobj     = mo.mavlink_object(mav_sock)           # 自動分配 socket_id
+    mobj.multiplexingToAnalysis = [0, 30, 32, 33, 74, 111, 147]  # HEARTBEAT + 常用訊息
+    mobj.run()

-    # 創建通道
-    connection_string="udp:127.0.0.1:14550"
-    mavlink_socket3 = mavutil.mavlink_connection(connection_string)
-    mavlink_object3 = mo.mavlink_object(mavlink_socket3)
-    # 設定通道流動
-    mavlink_object3.multiplexingToAnalysis = [0, 30, 32, 33, 74, 147]
-    mavlink_object3.multiplexingToReturn = [] # 
-    # mavlink_object3.multiplexingToSwap = [] #
-    # 啟動通道
-    mavlink_object3.run()
+    print("waiting for mavlink data ...")
+    time.sleep(2)   # 給一點時間收 HEARTBEAT

+    # 3) 顯示目前偵測到的 sysid 清單
+    print("Current sysid list:", list(bridge.mavlink_systems.keys()))

+    # 4) 主迴圈：持續將資料 emit 出去
+    try:
+        last_timesync = time.time()
+        while rclpy.ok():
+            now = time.time()

-    print('waiting for mavlink data ...')
-    time.sleep(2) # 等待 2 秒鐘 讓 device object 收到足夠的 mavlink 訊息
-
-    compid = 1   
-    sysid = 1
-    start_time = time.time()
-    analyzer.create_state(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    analyzer.create_attitude(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    analyzer.create_local_position_pose(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    analyzer.create_local_position_velocity(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    analyzer.create_global_global(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    analyzer.create_vfr_hud(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    analyzer.create_battery(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    analyzer.create_global_rel(sysid, analyzer.mavlink_systems[sysid].components[compid])
-    end_time = time.time()
-    print(f"Execution time for create all topic: {end_time - start_time} seconds")
-
-    print("start emit info")
-
-    start_time = time.time()
-    show_time = time.time() 
-    T = True
-    while T:
-        try:
-            # rclpy.spin(analyzer)
-            analyzer.emit_info() # 這邊是測試 node 的運行
+            # 每秒一次
+            if now - last_timesync >= 1.0:
+                # 發送 TIMESYNC request
+                mav_sock.mav.timesync_send(0, int(now * 1e9))
+                last_timesync = now
+                
+            bridge.emit_info()     # 將所有 emitParams 發布到 ROS topic
            time.sleep(0.5)
-        except KeyboardInterrupt:
-            break
-    
-    analyzer.destroy_node()
-    rclpy.shutdown()
-            
-    # 結束程式 退出所有 thread
-    mavlink_object3.stop()
-    mavlink_object3.thread.join()
-    analyzer.stop()
-    analyzer.thread.join()
+    except KeyboardInterrupt:
+        pass

-    mavlink_socket3.close()
-    print('<=== End of Program')
+    # -------- 清理 --------
+    bridge.destroy_node()
+    rclpy.shutdown()
+
+    mobj.stop();     mobj.thread.join()
+    bridge.stop();   bridge.thread.join()
+    mav_sock.close()
+    print("<=== End of Program")