Upload files to 'src/fc_network_adapter/fc_network_adapter'

src/fc_network_adapter/fc_network_adapter/mavlinkObject.py

@@ -20,6 +20,7 @@ from pymavlink import mavutil
 
 # ROS2 的 import
 from rclpy.node import Node
+import std_msgs.msg
 
 # 自定義的 import
 from mavlinkDevice import mavlink_device, mavlink_systems
@@ -117,6 +118,8 @@ class mavlink_bridge(Node, mavlink_publisher):
                 this_device.components[msg.get_srcComponent()] = this_component
                 this_component.mav_type = msg.type
                 this_component.mav_autopilot = msg.autopilot
+                this_component.sysid  = sysid     # ★ 新增
+                this_component.compid = compid 
             else:
                 continue
 
@@ -124,6 +127,7 @@ class mavlink_bridge(Node, mavlink_publisher):
 
             if msg_id == 0:  # HEARTBEAT 處理
                 this_component.emitParams['heartbeat'] = msg
+                self.ensure_all_publishers(sysid, this_component)
 
             elif msg_id == 30:  # ATTITUDE 處理
                 this_component.emitParams['attitude'] = msg
@@ -137,6 +141,12 @@ class mavlink_bridge(Node, mavlink_publisher):
             elif msg_id == 74:  # VFR_HUD 處理
                 this_component.emitParams['vfr_hud'] = msg
 
+            elif msg_id == 111: #TIME_SYNC
+                round_trip_time = std_msgs.msg.Float64()
+                round_trip_time.data = (int(time.time() * 1e9) - msg.ts1) / 1e6
+                if(round_trip_time.data < 1e6):
+                    this_component.emitParams['ping'] = round_trip_time
+
             elif msg_id == 147:  # BATTERY_STATUS 處理
                 this_component.emitParams['battery'] = msg
 
@@ -281,7 +291,6 @@ class mavlink_object():
     def _run_thread(self):
         logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
         logger.info('Start of mavlink_object._run_thread id : {}'.format(self.socket_id))
-        start_time = time.time()
         while self.running:
             timestamp = time.time() # 記錄接受到訊息的時間 # 這邊若是在大量訊息造成壅塞時 可能會有些微偏差
 
@@ -300,10 +309,21 @@ class mavlink_object():
                 # 統計收到的訊息 & 透過 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 類型
+                    device = self.mavlink_systems[sysid]
                     mavlink_systems[sysid].updateComponentPacketCount(compid, mavlinkMsg.get_seq(), mavlinkMsg.get_msgId(), timestamp)
                     
+                    comp = device.components.get(compid)
+                    if comp and comp.loss_rate_publisher:
+                        if timestamp - comp.last_loss_publish_time >= 1.0:
+                            loss_rate = comp.loss_rate  # loss_rate 是在 updateComponentPacketCount 中計算並儲存的欄位
+                            msg = std_msgs.msg.Float64()
+                            msg.data = float(loss_rate)
+                            comp.loss_rate_publisher.publish(msg)
+                            comp.last_loss_publish_time = timestamp
+
                 # break # Debug function can put here you can see the input data from mavlink
 
                 # 將訊息依照 multiplexing list 分發到不同的 queue