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

@@ -83,17 +83,16 @@ python gui.py
     建立、維護與飛控韌體的連接 
     構築 mavlink 封包
     處理無線模組的通訊格式 (XBee)
-    --同時處理與 Gazebo 的 ardupilot_plugin 溝通的 FDM/JSON 訊息 (移除)--
+2. fc_interfaces (必要)
-2. fc_interfaces (重要)
+    自定義的 ROS2 介面檔 沒有這個核心會運作不了
-    自定義的 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 

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):

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
 
@@ -116,6 +116,7 @@ class mavlink_bridge:
             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
+        # 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])  
+        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 訊息的處理機制
+
 '''
 

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位置
+            'summary': 0.0,         # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
-            'position_ned': 0.0,   # LOCAL_POSITION_NED (位置+速度)
+            '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 (地速 空速 絕對高度 爬升率 航向 油門)
+            'sys_diags': 0.0,       # SYS_STATUS 系統診斷
+            'status_text': 0.0,     # STATUSTEXT 飛控文字（佇列驅動，>0 僅作啟用旗標）
             'mode': 0.0,            # 飛行模式 (已經在 summary 裡 未來移除)
-            'summary': 0.0,        # 載具摘要 (sysid 飛行模式 解鎖上鎖 gps狀態)
+            'velocity': 0.0,        # 速度 (已經包含在 vfr_hud 未來移除)
-            'system_diagnostics': 0.0,  # SYS_STATUS 系統診斷
             # 在這裡新增更多 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
@@ -560,6 +568,14 @@ class MavlinkCommandService(Node):
     每次接到一個 service 請求 要整個系統丟某種指令給載具時
     會做兩件事 1."丟出mavlink封包" 2."創造一個臨時信箱 Pending"
     
+    然後透過每次 manager spin 
+    會去呼叫 return_router() 方法
+    這個方法會監聽 return_packet_ring 跟臨時信箱的 Pending 做配對
+    配對到的解開 Pending 
+
+    解開後 相對應的 handle_XXX 就會開始做事
+    
+
     """
 
     serviceString_prefix = '/fc_network/vehicle'
@@ -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 大量互動 也許需要考慮對方的生命週期
 

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