From 324fced754d21268431484c983030e39c153773e Mon Sep 17 00:00:00 2001 From: Chiyu Chen Date: Thu, 11 Jun 2026 07:50:50 +0800 Subject: [PATCH] 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