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