wenchun
/
AirTrapMine
forked from chiyu1468/AirTrapMine
1
0
Fork 0
Code Pull Requests Activity

Add SystemDiagnostics message and integrate into network adapter

Browse Source 
- 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.
Chiyu Chen 2 weeks ago
parent cc9d005392
commit 0a84bb68fe
7 changed files with 91 additions and 14 deletions
Show Stats Download Patch File Download Diff File

1
src/fc_interfaces/CMakeLists.txt
Unescape Escape View File

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

11
src/fc_interfaces/msg/SystemDiagnosticsRaw.msg
Unescape Escape View File

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

7
src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
Unescape Escape View File

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

19
src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
Unescape Escape View File

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

41
src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
Unescape Escape View File

@ -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，請在此處實作對應的發布方法

19
src/fc_network_adapter/fc_network_adapter/mavlinkVehicleView.py
Unescape Escape View File

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

7
src/unitdev02/unitdev02/devnote.txt
Unescape Escape View File

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

Write Preview
Loading…
Cancel
Save