From 2c7f2afc450ca66aac74fcf66a48d7bfa104d702 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Mon, 8 Jun 2026 23:09:14 +0800 Subject: [PATCH 1/4] (Add) ntrip_client.py GGA sentence generation and sending --- .../fc_network_module/ntrip_client.py | 111 +++++++++++++++--- 1 file changed, 94 insertions(+), 17 deletions(-) diff --git a/src/fc_network_module/fc_network_module/ntrip_client.py b/src/fc_network_module/fc_network_module/ntrip_client.py index 41ff58a..04b63d5 100644 --- a/src/fc_network_module/fc_network_module/ntrip_client.py +++ b/src/fc_network_module/fc_network_module/ntrip_client.py @@ -8,11 +8,65 @@ from rclpy.node import Node from rclpy.qos import QoSProfile, HistoryPolicy, ReliabilityPolicy, DurabilityPolicy from mavros_msgs.msg import RTCM +# TODO: ROS_DOMAIN_ID 要補一下 + +class GGA_stream(): + + @classmethod + def nmea_checksum(cls, body: str) -> str: + """body 不含 '$' 與 '*checksum'，例如 'GPGGA,123519,...'""" + value = 0 + for ch in body: + value ^= ord(ch) + return f"{value:02X}" + + @classmethod + def decimal_to_nmea_dm(cls, deg: float, *, is_latitude: bool) -> tuple[str, str]: + """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。""" + if is_latitude: + hemi = "N" if deg >= 0 else "S" + deg_width = 2 + else: + hemi = "E" if deg >= 0 else "W" + deg_width = 3 + deg = abs(deg) + d = int(deg) + m = (deg - d) * 60.0 + return f"{d:0{deg_width}d}{m:07.4f}", hemi + + @classmethod + def build_gga_sentence(cls, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> bytes: + """ + 組一筆 $GPGGA 句子（含 checksum) 回傳 bytes 可直接 sock.sendall。 + + lat_deg / lon_deg : 十進位經緯度(北、東為正)。 + 測試時請改成 mount 服務範圍內的近似位置；正式使用應來自 GNSS 真實定位。 + """ + utc = time.gmtime() + t_str = f"{utc.tm_hour:02d}{utc.tm_min:02d}{utc.tm_sec:02d}.00" + + lat_dm, ns = cls.decimal_to_nmea_dm(lat_deg, is_latitude=True) + lon_dm, ew = cls.decimal_to_nmea_dm(lon_deg, is_latitude=False) + + # quality=1（GPS fix）、8 顆星、HDOP=1.0 僅供測試示意 + body = ( + f"GPGGA,{t_str},{lat_dm},{ns},{lon_dm},{ew}," + f"1,08,1.0,{alt_m:.1f},M,0.0,M,," + ) + sentence = f"${body}*{cls.nmea_checksum(body)}\r\n" + return sentence.encode("ascii") + + @classmethod + def send_gga(cls, sock: socket.socket, lat_deg: float, lon_deg: float, alt_m: float = 100.0) -> str: + """送出 GGA，回傳可讀句子供列印。""" + payload = cls.build_gga_sentence(lat_deg, lon_deg, alt_m) + sock.sendall(payload) + return payload.decode("ascii").strip() class NtripClientNode(Node): - """連線 NTRIP caster，接收 RTCM3 資料流並發布至 ROS2 topic。""" + """連線 NTRIP caster, 接收 RTCM3 資料流並發布至 ROS2 topic """ - RECONNECT_BASE_SEC = 2.0 + RECONNECT_BASE_SEC = 10.0 RECONNECT_MAX_SEC = 60.0 def __init__(self): @@ -21,9 +75,15 @@ class NtripClientNode(Node): self.declare_parameter('host', 'rtk2go.com') self.declare_parameter('port', 2101) self.declare_parameter('mountpoint', '') - self.declare_parameter('username', '') + self.declare_parameter('username', 'template@mail.com') self.declare_parameter('password', '') + self.declare_parameter('gga_on', False) + self.declare_parameter('gga_lat', None) + self.declare_parameter('gga_lon', None) + self.declare_parameter('gga_alt_m', 100) + self.declare_parameter('gga_interval_sec', 300) + rtcm_qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, depth=1, @@ -43,28 +103,36 @@ class NtripClientNode(Node): def _receive_loop(self): backoff = self.RECONNECT_BASE_SEC + host = self.get_parameter('host').value + port = self.get_parameter('port').value + mountpoint = self.get_parameter('mountpoint').value + user = self.get_parameter('username').value + pwd = self.get_parameter('password').value + + if not mountpoint: + self.get_logger().error('mountpoint 參數未設定...') + return + while self._running: - host = self.get_parameter('host').value - port = self.get_parameter('port').value - mount = self.get_parameter('mountpoint').value - user = self.get_parameter('username').value - pwd = self.get_parameter('password').value - - if not mount: - self.get_logger().error('mountpoint 參數未設定，等待重試...') - time.sleep(backoff) - continue sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.settimeout(10) + self.gga_lat = self.get_parameter('gga_lat').value + self.gga_lon = self.get_parameter('gga_lon').value + self.gga_alt_m = self.get_parameter('gga_alt_m').value + self.gga_interval_sec = self.get_parameter('gga_interval_sec').value + self.gga_on = self.get_parameter('gga_on').value + self.gga_on = self.gga_lat is not None and self.gga_lon is not None and self.gga_on + + try: - self.get_logger().info(f'正在連線 {host}:{port}/{mount} ...') + self.get_logger().info(f'正在連線 {host}:{port}/{mountpoint} ...') sock.connect((host, port)) auth = base64.b64encode(f'{user}:{pwd}'.encode()).decode() request = ( - f'GET /{mount} HTTP/1.0\r\n' + f'GET /{mountpoint} HTTP/1.0\r\n' f'User-Agent: NTRIP PythonClient\r\n' f'Authorization: Basic {auth}\r\n' f'Connection: close\r\n\r\n' @@ -78,7 +146,9 @@ class NtripClientNode(Node): ) raise ConnectionError('handshake failed') - self.get_logger().info(f'已連接至 {mount}，開始接收 RTCM 資料流') + self.get_logger().info(f'已成功連接至 {mountpoint}') + + backoff = self.RECONNECT_BASE_SEC self._read_stream(sock) @@ -93,14 +163,21 @@ class NtripClientNode(Node): def _read_stream(self, sock: socket.socket): buf = b'' + last_gga_time = 0.0 while self._running: + now = time.time() + if self.gga_on and (now - last_gga_time >= self.gga_interval_sec): + line = GGA_stream.send_gga(sock, self.gga_lat, self.gga_lon, self.gga_alt_m) + self.get_logger().info(f"[GGA] {line}") + last_gga_time = now + try: chunk = sock.recv(4096) except socket.timeout: continue if not chunk: - break + raise ConnectionError("stream ended. Empty Chunk") buf += chunk while len(buf) >= 6: From cc9d00539257ebaf82584334ef437666cae13d3b Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Tue, 9 Jun 2026 14:06:44 +0800 Subject: [PATCH 2/4] (update) ntrip_client.py Introduced threading locks and stop event for safer socket management. Updated the receive loop to handle connection interruptions. --- .../fc_network_module/ntrip_client.py | 57 +++++++++++++++---- src/unitdev02/unitdev02/devnote.txt | 2 + 2 files changed, 47 insertions(+), 12 deletions(-) diff --git a/src/fc_network_module/fc_network_module/ntrip_client.py b/src/fc_network_module/fc_network_module/ntrip_client.py index 04b63d5..6d4e8c2 100644 --- a/src/fc_network_module/fc_network_module/ntrip_client.py +++ b/src/fc_network_module/fc_network_module/ntrip_client.py @@ -2,6 +2,7 @@ import socket import base64 import threading import time +from typing import Optional, Tuple import rclpy from rclpy.node import Node @@ -21,7 +22,7 @@ class GGA_stream(): return f"{value:02X}" @classmethod - def decimal_to_nmea_dm(cls, deg: float, *, is_latitude: bool) -> tuple[str, str]: + def decimal_to_nmea_dm(cls, deg: float, *, is_latitude: bool) -> Tuple[str, str]: """十進位度 → NMEA 的 (d)dmm.mmmm 與半球字元。""" if is_latitude: hemi = "N" if deg >= 0 else "S" @@ -82,7 +83,7 @@ class NtripClientNode(Node): self.declare_parameter('gga_lat', None) self.declare_parameter('gga_lon', None) self.declare_parameter('gga_alt_m', 100) - self.declare_parameter('gga_interval_sec', 300) + self.declare_parameter('gga_interval_sec', 60) rtcm_qos = QoSProfile( history=HistoryPolicy.KEEP_LAST, @@ -92,7 +93,9 @@ class NtripClientNode(Node): ) self.publisher_ = self.create_publisher(RTCM, '/fc_network/rtcm_input', rtcm_qos) - self._running = True + self._stop_event = threading.Event() + self._sock_lock = threading.Lock() + self._active_sock: Optional[socket.socket] = None self._thread = threading.Thread(target=self._receive_loop, daemon=True) self._thread.start() @@ -100,6 +103,15 @@ class NtripClientNode(Node): # ── NTRIP 連線 + RTCM 接收迴圈 ────────────────────────────── + def _request_stop(self): + self._stop_event.set() + with self._sock_lock: + if self._active_sock is not None: + try: + self._active_sock.close() + except OSError: + pass + def _receive_loop(self): backoff = self.RECONNECT_BASE_SEC @@ -113,10 +125,12 @@ class NtripClientNode(Node): self.get_logger().error('mountpoint 參數未設定...') return - while self._running: + while not self._stop_event.is_set(): sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) sock.settimeout(10) + with self._sock_lock: + self._active_sock = sock self.gga_lat = self.get_parameter('gga_lat').value self.gga_lon = self.get_parameter('gga_lon').value @@ -154,18 +168,26 @@ class NtripClientNode(Node): self._read_stream(sock) except Exception as e: - if self._running: - self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連') - time.sleep(backoff) - backoff = min(backoff * 2, self.RECONNECT_MAX_SEC) + if self._stop_event.is_set(): + break + self.get_logger().warn(f'連線中斷: {e}，{backoff:.0f}s 後重連') + if self._stop_event.wait(timeout=backoff): + break + backoff = min(backoff * 2, self.RECONNECT_MAX_SEC) finally: - sock.close() + with self._sock_lock: + if self._active_sock is sock: + self._active_sock = None + try: + sock.close() + except OSError: + pass def _read_stream(self, sock: socket.socket): buf = b'' last_gga_time = 0.0 - while self._running: + while not self._stop_event.is_set(): now = time.time() if self.gga_on and (now - last_gga_time >= self.gga_interval_sec): line = GGA_stream.send_gga(sock, self.gga_lat, self.gga_lon, self.gga_alt_m) @@ -175,8 +197,16 @@ class NtripClientNode(Node): try: chunk = sock.recv(4096) except socket.timeout: + if self._stop_event.is_set(): + break continue + except OSError: + if self._stop_event.is_set(): + break + raise if not chunk: + if self._stop_event.is_set(): + break raise ConnectionError("stream ended. Empty Chunk") buf += chunk @@ -203,8 +233,11 @@ class NtripClientNode(Node): self.publisher_.publish(msg) def destroy_node(self): - self._running = False - self._thread.join(timeout=3.0) + self._request_stop() + if self._thread.is_alive(): + self._thread.join(timeout=5.0) + if self._thread.is_alive(): + self.get_logger().warn('receive thread 未在時限內結束') super().destroy_node() diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt index df3e429..513e734 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/src/unitdev02/unitdev02/devnote.txt @@ -34,6 +34,8 @@ python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p mountpo python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart +python3 -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart -p gga_on:=True -p gga_lat:=24.155792000 -p gga_lon:=120.630679000 -p gga_interval_sec:=60 + export ROS_DOMAIN_ID=13 ros2 daemon stop ros2 daemon start From 0a84bb68fe9698d3e17b8f5824452b92567020b0 Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Thu, 11 Jun 2026 02:50:33 +0800 Subject: [PATCH 3/4] Add SystemDiagnostics message and integrate into network adapter - Added SystemDiagnosticsRaw.msg to define system diagnostics data structure. - Enhanced mainOrchestrator.py and mavlinkObject.py to handle SYS_STATUS messages and publish system diagnostics. - Introduced a new method in VehicleStatusPublisher to publish system diagnostics data. - Updated mavlinkVehicleView.py to include SystemDiagnostics data class. --- src/fc_interfaces/CMakeLists.txt | 1 + .../msg/SystemDiagnosticsRaw.msg | 11 +++++ .../fc_network_adapter/mainOrchestrator.py | 7 ++-- .../fc_network_adapter/mavlinkObject.py | 19 ++++++++- .../fc_network_adapter/mavlinkROS2Nodes.py | 41 ++++++++++++++++--- .../fc_network_adapter/mavlinkVehicleView.py | 19 ++++++++- src/unitdev02/unitdev02/devnote.txt | 7 +++- 7 files changed, 91 insertions(+), 14 deletions(-) create mode 100644 src/fc_interfaces/msg/SystemDiagnosticsRaw.msg diff --git a/src/fc_interfaces/CMakeLists.txt b/src/fc_interfaces/CMakeLists.txt index 1fe91d1..906d9c9 100644 --- a/src/fc_interfaces/CMakeLists.txt +++ b/src/fc_interfaces/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(builtin_interfaces REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/AttitudeRaw.msg" "msg/GnssRaw.msg" + "msg/SystemDiagnosticsRaw.msg" "msg/ServiceAckResult.msg" "srv/MavPing.srv" "srv/MavCommandLong.srv" diff --git a/src/fc_interfaces/msg/SystemDiagnosticsRaw.msg b/src/fc_interfaces/msg/SystemDiagnosticsRaw.msg new file mode 100644 index 0000000..7bfeaeb --- /dev/null +++ b/src/fc_interfaces/msg/SystemDiagnosticsRaw.msg @@ -0,0 +1,11 @@ +builtin_interfaces/Time stamp +uint32 sensors_install_mask +uint32 sensors_enabled_mask +uint32 sensors_health_mask +uint16 mcu_load +uint16 bus_error_rate +uint16 bus_error_count +uint16 errors_count1 +uint16 errors_count2 +uint16 errors_count3 +uint16 errors_count4 diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 07c86d7..9fcf4b4 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -29,7 +29,7 @@ from .utils import acquireSerial, acquirePort from .utils.acquirePort import find_available_port logger = setup_logger(os.path.basename(__file__)) -PROJECT_VER = "v1.10" +PROJECT_VER = "v1.20" class PanelState: def __init__(self): @@ -1198,6 +1198,7 @@ class Orchestrator: 'vfr_hud': 1.0, 'mode': 0.0, 'summary': 1.0, + 'system_diagnostics': 1.0, } def engageWholeSystem(self): @@ -1642,10 +1643,10 @@ def main(): if mo.MODULE_VER != "1.50": print("Module Version Error! : mavlinkObkect") version_check = False - if mros.MODULE_VER != "2.10": + if mros.MODULE_VER != "2.50": print("Module Version Error! : mavlinkROS2Nodes") version_check = False - if mvv.MODULE_VER != "1.00": + if mvv.MODULE_VER != "1.10": print("Module Version Error! : mavlinkVehicleView") version_check = False if sm.MODULE_VER != "0.80": diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index ab3e80f..6556e19 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -109,6 +109,7 @@ class mavlink_bridge: """初始化訊息處理器映射表，提高處理效率""" self.message_handlers = { 0: self._handle_heartbeat, # HEARTBEAT + 1: self._handle_vehicle_sys_status, # SYS_STATUS 24: self._handle_gps_raw_int, # GPS_RAW_INT 30: self._handle_attitude, # ATTITUDE 32: self._handle_local_position, # LOCAL_POSITION_NED @@ -231,6 +232,20 @@ class mavlink_bridge: 'vx': msg.vx, 'vy': msg.vy, 'vz': msg.vz } + def _handle_vehicle_sys_status(self, vehicle, component, msg, timestamp): + """處理 SYS_STATUS 訊息 (msg_id: 1)""" + diag = component.status.sys_diag + diag.sensors_install_mask = msg.onboard_control_sensors_present + diag.sensors_enabled_mask = msg.onboard_control_sensors_enabled + diag.sensors_health_mask = msg.onboard_control_sensors_health + diag.mcuLoad = msg.load + diag.busErrorRate = msg.drop_rate_comm + diag.busErrorCount = msg.errors_comm + diag.errors_count1 = msg.errors_count1 + diag.errors_count2 = msg.errors_count2 + diag.errors_count3 = msg.errors_count3 + diag.errors_count4 = msg.errors_count4 + diag.timestamp = timestamp def _handle_global_position(self, vehicle, component, msg, timestamp): """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)""" @@ -395,9 +410,9 @@ class mavlink_object: self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191) # 記錄訊息過濾類型 (可選) - # 0 HEARTBEAT, 24 GPS_RAW_INT, 30 ATTITUDE, + # 0 HEARTBEAT, 1 SYS_STATUS, 24 GPS_RAW_INT, 30 ATTITUDE, # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS - self.bridge_msg_types = set([0, 24, 30, 32, 33, 74, 147]) + self.bridge_msg_types = set([0, 1, 24, 30, 32, 33, 74, 147]) self.return_msg_types = set([]) # 轉發到別的 mavlink object 作為目標端口 的列表 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index 6948673..eaebd90 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -45,7 +45,7 @@ from . import mavlinkObject as mo from .utils import setup_logger logger = setup_logger(os.path.basename(__file__)) -MODULE_VER = "2.10" +MODULE_VER = "2.50" FC_ROS_DOMAIN_ID = "0" NODE_KEYS = ("status_publisher", "command_service", "rtcm_relay") @@ -76,6 +76,7 @@ class PublishRateController: 'vfr_hud': 0.0, # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門) 'mode': 0.0, # 飛行模式 (已經在 summary 裡 未來移除) 'summary': 0.0, # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態) + 'system_diagnostics': 0.0, # SYS_STATUS 系統診斷 # 在這裡新增更多 topics... } # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp} @@ -186,6 +187,7 @@ class VehicleStatusPublisher(Node): self._publish_vfr_hud(sysid, status) self._publish_mode(sysid, status) self._publish_summary(vehicle) + self._publish_system_diagnostics(sysid, status) # 在這裡新增更多 publish 方法調用... def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1): @@ -439,11 +441,7 @@ class VehicleStatusPublisher(Node): 'armed': status.armed if status.armed is not None else False, # 'mode_custom': status.mode.custom_mode if status.mode.custom_mode else 0, 'mode_name': status.mode.mode_name if status.mode.mode_name else "UNKNOWN", - # 'latitude': status.position.latitude if status.position.latitude else 0.0, - # 'longitude': status.position.longitude if status.position.longitude else 0.0, - # 'altitude': status.position.altitude if status.position.altitude else 0.0, - # 'battery_percent': status.battery.remaining if status.battery.remaining else 0, - # 'gps_fix': status.gps.fix_type if status.gps.fix_type else 0, + 'mav_status': status.system_status if status.system_status is not None else 0, 'connection_type': vehicle.connected_via.value, 'last_update': component.packet_stats.last_msg_time if component.packet_stats.last_msg_time else 0.0, } @@ -452,6 +450,37 @@ class VehicleStatusPublisher(Node): msg.data = json.dumps(summary) publisher.publish(msg) + def _publish_system_diagnostics(self, sysid: int, status: mvv.ComponentStatus): + """發布 SYS_STATUS 系統診斷資訊""" + if not self.rate_controller.should_publish(sysid, 'system_diagnostics'): + return + + diag = status.sys_diag + if diag.sensors_install_mask is None: + return + + publisher = self._get_or_create_publisher( + sysid, 'system_diagnostics', fcmsg.SystemDiagnosticsRaw + ) + + if publisher.get_subscription_count() == 0: + return + + msg = fcmsg.SystemDiagnosticsRaw() + msg.stamp = self.get_clock().now().to_msg() + msg.sensors_install_mask = int(diag.sensors_install_mask) + msg.sensors_enabled_mask = int(diag.sensors_enabled_mask or 0) + msg.sensors_health_mask = int(diag.sensors_health_mask or 0) + msg.mcu_load = int(diag.mcuLoad or 0) + msg.bus_error_rate = int(diag.busErrorRate or 0) + msg.bus_error_count = int(diag.busErrorCount or 0) + msg.errors_count1 = int(diag.errors_count1 or 0) + msg.errors_count2 = int(diag.errors_count2 or 0) + msg.errors_count3 = int(diag.errors_count3 or 0) + msg.errors_count4 = int(diag.errors_count4 or 0) + + publisher.publish(msg) + # ═══════════════════════════════════════════════════════════════ # 【新增 Topic 位置 3/4】 # 若要新增 topic，請在此處實作對應的發布方法 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py index 81dbbf9..3578f53 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -12,7 +12,7 @@ from enum import Enum from .utils import setup_logger logger = setup_logger(os.path.basename(__file__)) -MODULE_VER = "1.00" +MODULE_VER = "1.10" # ====================== Enums ===================== @@ -117,6 +117,22 @@ class VFR: timestamp: Optional[float] = None # 時間戳記 +@dataclass +class SystemDiagnostics: + """系統診斷資訊（來源：MAVLink SYS_STATUS，不含電池欄位）""" + sensors_install_mask: Optional[int] = None + sensors_enabled_mask: Optional[int] = None + sensors_health_mask: Optional[int] = None + mcuLoad: Optional[int] = None # 單位 1% + busErrorRate: Optional[int] = None # 單位 0.1% + busErrorCount: Optional[int] = None + errors_count1: Optional[int] = None + errors_count2: Optional[int] = None + errors_count3: Optional[int] = None + errors_count4: Optional[int] = None + timestamp: Optional[float] = None + + @dataclass class ComponentStatus: """組件狀態容器""" @@ -127,6 +143,7 @@ class ComponentStatus: ekf: EKF = field(default_factory=EKF) gps: GPS = field(default_factory=GPS) vfr: VFR = field(default_factory=VFR) + sys_diag: SystemDiagnostics = field(default_factory=SystemDiagnostics) # 系統狀態 system_status: Optional[int] = None # MAV_STATE diff --git a/src/unitdev02/unitdev02/devnote.txt b/src/unitdev02/unitdev02/devnote.txt index 513e734..1f22c69 100644 --- a/src/unitdev02/unitdev02/devnote.txt +++ b/src/unitdev02/unitdev02/devnote.txt @@ -8,7 +8,8 @@ RTK 跟 RTK2go 抓取列表 Done 從特定 mount point 得到數據 Done - 做一個 ros2 service 接到數據並包裝 + 做一個 ros2 service 接到數據並包裝 Done + RTK 實機測試 下一步 @@ -36,7 +37,9 @@ python -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=2 python3 -m fc_network_module.fc_network_module.ntrip_client --ros-args -p host:=210.241.63.193 -p port:=81 -p mountpoint:=2020_GNSS -p username:=uavlab6061 -p password:=iamsupersmart -p gga_on:=True -p gga_lat:=24.155792000 -p gga_lon:=120.630679000 -p gga_interval_sec:=60 -export ROS_DOMAIN_ID=13 +watch GPS_RAW_INT + +export ROS_DOMAIN_ID=1 ros2 daemon stop ros2 daemon start From 324fced754d21268431484c983030e39c153773e Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Thu, 11 Jun 2026 07:50:50 +0800 Subject: [PATCH 4/4] Enhance mavlink handling and add status text support - Added handling for SYS_STATUS and STATUSTEXT messages in mainOrchestrator.py and mavlinkObject.py. - Introduced a new StatusTextEntry data class in mavlinkVehicleView.py to manage status text messages. - Updated topic management in mavlinkROS2Nodes.py to include new status text functionality. --- README.md | 9 +- .../fc_network_adapter/mainOrchestrator.py | 5 +- .../fc_network_adapter/mavlinkObject.py | 33 +++++-- .../fc_network_adapter/mavlinkROS2Nodes.py | 98 ++++++++++++------- .../fc_network_adapter/mavlinkVehicleView.py | 10 ++ 5 files changed, 101 insertions(+), 54 deletions(-) diff --git a/README.md b/README.md index dd27db2..2684f10 100644 --- a/README.md +++ b/README.md @@ -83,17 +83,16 @@ python gui.py 建立、維護與飛控韌體的連接 構築 mavlink 封包 處理無線模組的通訊格式 (XBee) - --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)-- -2. fc_interfaces (重要) - 自定義的 ROS2 介面檔 沒啥好說的 沒有這個核心會運作不了 +2. fc_interfaces (必要) + 自定義的 ROS2 介面檔 沒有這個核心會運作不了 3. fc_network_module (重要) 非核心 但是支援載具的重要附屬功能 需要該功能時 作為一個 ros2 節點打開 例如 : ntrip rtk 訊號轉接 4. fc_network_apps - 與 fc_network_adapter 銜接做高階功能包裝的應用小程式 + 是 fc_network_adapter 高階包裝API 利於開發GUI或其他應用 使用者的外層包裝 - 這裡的定位是 "核心功能的高階包裝" 可以完全不去用 + 可以不使用 或者當作一個範例程式來看 5. someotherpkg 如何使用 fc_network_apps 的範例檔案 6. GUI diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py index 9fcf4b4..67bc1f4 100644 --- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py +++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py @@ -1106,7 +1106,7 @@ class ControlPanel: 0: "HB", 1: "S_STAT", 2: "S_TIME", 24: "GPS_RAW", 27: "RAW_IMU", 30: "ATT", 32: "LOC_POS", 33: "GLB_POS", 62: "NAV_CTL", 74: "VFR_HUD", 147: "BATT_ST", 136: "TERRAIN", 241: "VIBRAT", - 125: "POW_STA", + 125: "POW_STA", 253: "STAT_TXT", } # ardupilot mega @@ -1198,7 +1198,8 @@ class Orchestrator: 'vfr_hud': 1.0, 'mode': 0.0, 'summary': 1.0, - 'system_diagnostics': 1.0, + 'sys_diags': 1.0, + 'status_text': 1.0, } def engageWholeSystem(self): diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py index 6556e19..f1ab903 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py @@ -51,7 +51,7 @@ from .mavlinkVehicleView import ( VehicleView, VehicleComponent, ComponentType, - ConnectionType + StatusTextEntry, ) from .utils import RingBuffer, setup_logger @@ -108,14 +108,15 @@ class mavlink_bridge: def _init_message_handlers(self): """初始化訊息處理器映射表，提高處理效率""" self.message_handlers = { - 0: self._handle_heartbeat, # HEARTBEAT + 0: self._handle_heartbeat, # HEARTBEAT 1: self._handle_vehicle_sys_status, # SYS_STATUS - 24: self._handle_gps_raw_int, # GPS_RAW_INT - 30: self._handle_attitude, # ATTITUDE - 32: self._handle_local_position, # LOCAL_POSITION_NED - 33: self._handle_global_position, # GLOBAL_POSITION_INT - 74: self._handle_vfr_hud, # VFR_HUD - 147: self._handle_battery_status, # BATTERY_STATUS + 24: self._handle_gps_raw_int, # GPS_RAW_INT + 30: self._handle_attitude, # ATTITUDE + 32: self._handle_local_position, # LOCAL_POSITION_NED + 33: self._handle_global_position, # GLOBAL_POSITION_INT + 74: self._handle_vfr_hud, # VFR_HUD + 147: self._handle_battery_status, # BATTERY_STATUS + 253: self._handle_status_text, # STATUSTEXT } def start(self): @@ -247,6 +248,15 @@ class mavlink_bridge: diag.errors_count4 = msg.errors_count4 diag.timestamp = timestamp + def _handle_status_text(self, vehicle, component, msg, timestamp): + """處理 STATUSTEXT 訊息 (msg_id: 253)""" + text = msg.text.rstrip('\x00') if msg.text else '' + if not text: + return + component.status.status_text_queue.append( + StatusTextEntry(text=text, severity=msg.severity, timestamp=timestamp) + ) + def _handle_global_position(self, vehicle, component, msg, timestamp): """處理 GLOBAL_POSITION_INT 訊息 (msg_id: 33)""" component.status.position.latitude = msg.lat / 1e7 # 轉換為度 @@ -411,8 +421,8 @@ class mavlink_object: # 記錄訊息過濾類型 (可選) # 0 HEARTBEAT, 1 SYS_STATUS, 24 GPS_RAW_INT, 30 ATTITUDE, - # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS - self.bridge_msg_types = set([0, 1, 24, 30, 32, 33, 74, 147]) + # 32 LOCAL_POSITION_NED, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 253 STATUSTEXT + self.bridge_msg_types = set([0, 1, 24, 30, 32, 33, 74, 147, 253]) self.return_msg_types = set([]) # 轉發到別的 mavlink object 作為目標端口 的列表 @@ -849,5 +859,8 @@ if __name__ == '__main__': 1. async_io_manager.managed_objects 與 mavlink_object.mavlinkObjects 功能重複整合 保留 mavlink_object.mavlinkObjects 2. async_io_manager 的 _stop_event 無效變數移除 +2026年 06月 10日 +1. 增加 SYS_STATUS 與 STATUSTEXT 訊息的處理機制 + ''' diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py index eaebd90..34e9828 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py @@ -68,15 +68,16 @@ class PublishRateController: # 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉 # 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段 self.topic_intervals = { - 'position_gnss': 0.0, # GNSS位置 - 'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度) - 'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態) - 'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除) - 'battery': 0.0, # 電池 - 'vfr_hud': 0.0, # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門) - 'mode': 0.0, # 飛行模式 (已經在 summary 裡 未來移除) - 'summary': 0.0, # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態) - 'system_diagnostics': 0.0, # SYS_STATUS 系統診斷 + 'summary': 0.0, # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態) + 'position_gnss': 0.0, # GNSS位置 (海拔高度) + 'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度+相對高度) + 'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態) + 'battery': 0.0, # 電池 + 'vfr_hud': 0.0, # VFR HUD (地速 空速 絕對高度 爬升率 航向 油門) + 'sys_diags': 0.0, # SYS_STATUS 系統診斷 + 'status_text': 0.0, # STATUSTEXT 飛控文字（佇列驅動，>0 僅作啟用旗標） + 'mode': 0.0, # 飛行模式 (已經在 summary 裡 未來移除) + 'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除) # 在這裡新增更多 topics... } # 記錄每個 topic 的最後發布時間 {(sysid, topic): timestamp} @@ -113,6 +114,10 @@ class PublishRateController: return False + def is_topic_enabled(self, topic: str) -> bool: + """檢查 topic 是否啟用（interval > 0）""" + return self.topic_intervals.get(topic, 0) > 0 + def reset(self): """重置所有計時器""" self.last_publish_time.clear() @@ -123,7 +128,7 @@ class VehicleStatusPublisher(Node): 職責: - 定期從 vehicle_registry 讀取載具狀態 - - 頻率控制（位置/姿態 2Hz，電池/摘要 1Hz） + - 頻率控制 (位置/姿態 2Hz, 電池/摘要 1Hz) - 發布標準 ROS2 消息類型 - 檢測訂閱者，按需發布 """ @@ -182,12 +187,13 @@ class VehicleStatusPublisher(Node): self._publish_position_gnss(sysid, status) self._publish_position_ned(sysid, status) self._publish_attitude(sysid, status) - self._publish_velocity(sysid, status) self._publish_battery(sysid, status) self._publish_vfr_hud(sysid, status) - self._publish_mode(sysid, status) self._publish_summary(vehicle) self._publish_system_diagnostics(sysid, status) + self._publish_status_text(sysid, status) + self._publish_velocity(sysid, status) + self._publish_mode(sysid, status) # 在這裡新增更多 publish 方法調用... def _get_or_create_publisher(self, sysid: int, topic: str, msg_type, qos: int = 1): @@ -452,7 +458,7 @@ class VehicleStatusPublisher(Node): def _publish_system_diagnostics(self, sysid: int, status: mvv.ComponentStatus): """發布 SYS_STATUS 系統診斷資訊""" - if not self.rate_controller.should_publish(sysid, 'system_diagnostics'): + if not self.rate_controller.should_publish(sysid, 'sys_diags'): return diag = status.sys_diag @@ -460,7 +466,7 @@ class VehicleStatusPublisher(Node): return publisher = self._get_or_create_publisher( - sysid, 'system_diagnostics', fcmsg.SystemDiagnosticsRaw + sysid, 'sys_diags', fcmsg.SystemDiagnosticsRaw ) if publisher.get_subscription_count() == 0: @@ -481,6 +487,31 @@ class VehicleStatusPublisher(Node): publisher.publish(msg) + def _publish_status_text(self, sysid: int, status: mvv.ComponentStatus): + """發布 STATUSTEXT 飛控文字 (佇列 drain, 無訂閱者直接丟棄) """ + # 是否啟用 + if not self.rate_controller.is_topic_enabled('status_text'): + return + # 是否有資料 + queue = status.status_text_queue + if not queue: + return + + publisher = self._get_or_create_publisher(sysid, 'status_text', std_msgs.msg.String) + + # 是否有監聽者 + if publisher.get_subscription_count() == 0: + queue.clear() + return + + while queue: + entry = queue.popleft() + msg = std_msgs.msg.String() + ts = entry.timestamp if entry.timestamp is not None else 0.0 + sev = entry.severity if entry.severity is not None else -1 + msg.data = f'[{ts:.3f}] [{sev}] {entry.text}' + publisher.publish(msg) + # ═══════════════════════════════════════════════════════════════ # 【新增 Topic 位置 3/4】 # 若要新增 topic，請在此處實作對應的發布方法 @@ -499,29 +530,6 @@ class VehicleStatusPublisher(Node): # # ... 實作發布邏輯 # ═══════════════════════════════════════════════════════════════ - @staticmethod - def _euler_to_quaternion(roll, pitch, yaw): - """ - 歐拉角轉四元數 - - Args: - roll: 橫滾角 (弧度) - pitch: 俯仰角 (弧度) - yaw: 偏航角 (弧度) - - Returns: - tuple: (qx, qy, qz, qw) - """ - qx = math.sin(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) - \ - math.cos(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) - qy = math.cos(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) + \ - math.sin(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - qz = math.cos(roll/2) * math.cos(pitch/2) * math.sin(yaw/2) - \ - math.sin(roll/2) * math.sin(pitch/2) * math.cos(yaw/2) - qw = math.cos(roll/2) * math.cos(pitch/2) * math.cos(yaw/2) + \ - math.sin(roll/2) * math.sin(pitch/2) * math.sin(yaw/2) - return (qx, qy, qz, qw) - def stop(self): """停止發布""" self.running = False @@ -559,6 +567,14 @@ class MavlinkCommandService(Node): 講白話一點就是 每次接到一個 service 請求 要整個系統丟某種指令給載具時 會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending" + + 然後透過每次 manager spin + 會去呼叫 return_router() 方法 + 這個方法會監聽 return_packet_ring 跟臨時信箱的 Pending 做配對 + 配對到的解開 Pending + + 解開後 相對應的 handle_XXX 就會開始做事 + """ @@ -1182,6 +1198,10 @@ class fc_ros_manager: - RtcmRelay 提供統一的啟動/停止介面給 mainOrchestrator + + 另外 這邊用到 MultiThreadedExecutor 會開出額外的 thread 的特性 + 使得就算 executor 在跑一些需要等待的方法 + 常態的 spin_once 也不會被 block (spin_thread 是另一個支線) """ def __init__(self): @@ -1461,6 +1481,10 @@ ros2_manager = fc_ros_manager() 2. schedule_restart_node / _restart_node : 手動重啟單一 node (spin thread 內執行） 3. orchestrator cmd: ("RESTART_ROS_NODE", node_key), node_key 見 NODE_KEYS +2026.06.10 +1. 增加了 _publish_system_diagnostics 與 _publish_status_text 功能 + + TODO 1. service 部分會需要跟 mavlinkobject 大量互動 也許需要考慮對方的生命週期 diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py index 3578f53..89b3cb4 100644 --- a/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py +++ b/src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py @@ -5,6 +5,7 @@ VehicleView - Pure State Container """ import os +from collections import deque from typing import Dict, Optional, Any, Tuple from dataclasses import dataclass, field from enum import Enum @@ -117,6 +118,14 @@ class VFR: timestamp: Optional[float] = None # 時間戳記 +@dataclass +class StatusTextEntry: + """飛控狀態文字（來源：MAVLink STATUSTEXT）""" + text: str + severity: Optional[int] = None + timestamp: Optional[float] = None + + @dataclass class SystemDiagnostics: """系統診斷資訊（來源：MAVLink SYS_STATUS，不含電池欄位）""" @@ -144,6 +153,7 @@ class ComponentStatus: gps: GPS = field(default_factory=GPS) vfr: VFR = field(default_factory=VFR) sys_diag: SystemDiagnostics = field(default_factory=SystemDiagnostics) + status_text_queue: deque = field(default_factory=lambda: deque(maxlen=64)) # 系統狀態 system_status: Optional[int] = None # MAV_STATE