diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 0a219be..88046b8 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/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,9 +309,20 @@ 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