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

@@ -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"

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

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

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 作為目標端口 的列表

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,
+            'mav_status': status.system_status if status.system_status is not None else 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,
             '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，請在此處實作對應的發布方法

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

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