diff --git a/src/fc_interfaces/CMakeLists.txt b/src/fc_interfaces/CMakeLists.txt
index d86950e..36b3500 100644
--- a/src/fc_interfaces/CMakeLists.txt
+++ b/src/fc_interfaces/CMakeLists.txt
@@ -9,13 +9,15 @@ endif()
find_package(ament_cmake REQUIRED)
find_package(rosidl_default_generators REQUIRED)
find_package(std_msgs REQUIRED)
+find_package(builtin_interfaces REQUIRED)
rosidl_generate_interfaces(${PROJECT_NAME}
"msg/AttitudeRaw.msg"
+ "msg/GnssRaw.msg"
"srv/MavPing.srv"
"srv/MavCommandLong.srv"
"srv/MavPositionTargetGlobalInt.srv"
- DEPENDENCIES std_msgs
+ DEPENDENCIES std_msgs builtin_interfaces
)
ament_package()
diff --git a/src/fc_interfaces/msg/GnssRaw.msg b/src/fc_interfaces/msg/GnssRaw.msg
new file mode 100644
index 0000000..2ab69bc
--- /dev/null
+++ b/src/fc_interfaces/msg/GnssRaw.msg
@@ -0,0 +1,8 @@
+builtin_interfaces/Time stamp
+float64 latitude
+float64 longitude
+float64 altitude
+uint8 fix_type
+uint8 satellites_visible
+float32 eph
+float32 epv
diff --git a/src/fc_interfaces/package.xml b/src/fc_interfaces/package.xml
index 5dcd350..a215b5c 100644
--- a/src/fc_interfaces/package.xml
+++ b/src/fc_interfaces/package.xml
@@ -17,6 +17,8 @@
std_msgs
std_msgs
+ builtin_interfaces
+ builtin_interfaces
ament_cmake
diff --git a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
index dd0b36b..37c8b42 100644
--- a/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
+++ b/src/fc_network_adapter/fc_network_adapter/mainOrchestrator.py
@@ -1145,7 +1145,7 @@ class Orchestrator:
self.fc_ros_manager = mros.ros2_manager
self.fc_ros_manager.initialize()
self.fc_ros_manager.status_publisher.rate_controller.topic_intervals = {
- 'position': 1.0,
+ 'position_gnss': 1.0,
'position_ned': 1.0,
'attitude': 1.0,
'velocity': 0.0,
diff --git a/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py b/src/fc_network_adapter/fc_network_adapter/mavlinkObject.py
index 15dcef6..ab3e80f 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
+ 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
@@ -239,6 +240,14 @@ class mavlink_bridge:
component.status.position.relative_altitude = msg.relative_alt / 1000.0 # 轉換為公尺
component.status.position.timestamp = timestamp
+ def _handle_gps_raw_int(self, vehicle, component, msg, timestamp):
+ """處理 GPS_RAW_INT 訊息 (msg_id: 24)"""
+ component.status.gps.fix_type = msg.fix_type
+ component.status.gps.satellites_visible = msg.satellites_visible
+ component.status.gps.eph = None if msg.eph == 65535 else msg.eph
+ component.status.gps.epv = None if msg.epv == 65535 else msg.epv
+ component.status.gps.timestamp = timestamp
+
def _handle_vfr_hud(self, vehicle, component, msg, timestamp):
"""處理 VFR_HUD 訊息 (msg_id: 74)"""
component.status.vfr.airspeed = msg.airspeed
@@ -386,7 +395,9 @@ class mavlink_object:
self.MAVLink = mav_common.MAVLink(self.mavlinkPipeline, srcSystem=254, srcComponent=191)
# 記錄訊息過濾類型 (可選)
- self.bridge_msg_types = set([0, 30, 32, 33, 74, 147]) # 0 HEARTBEAT, 30 ATTITUDE, 33 GLOBAL_POSITION_INT, 74 VFR_HUD, 147 BATTERY_STATUS, 32 LOCAL_POSITION_NED
+ # 0 HEARTBEAT, 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.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 96455fc..5e03fc2 100644
--- a/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
+++ b/src/fc_network_adapter/fc_network_adapter/mavlinkROS2Nodes.py
@@ -61,7 +61,7 @@ class PublishRateController:
# 注意 這邊是定義區 不要把參數寫在這裡 所以預設全部關閉
# 以這個專案 請看 mainOrchestrator.py 的 Orchestrator 初始化階段
self.topic_intervals = {
- 'position': 0.0, # GNSS位置
+ 'position_gnss': 0.0, # GNSS位置
'position_ned': 0.0, # LOCAL_POSITION_NED (位置+速度)
'attitude': 0.0, # 姿態 (pitch yaw row 與其加速狀態)
'velocity': 0.0, # 速度 (已經包含在 vfr_hud 未來移除)
@@ -171,7 +171,7 @@ class VehicleStatusPublisher(Node):
# 例如：self._publish_ekf_status(sysid, status)
# ═══════════════════════════════════════════════════════════════
# 發布各種狀態（通過頻率控制器判斷是否發布）
- self._publish_position(sysid, status)
+ self._publish_position_gnss(sysid, status)
self._publish_position_ned(sysid, status)
self._publish_attitude(sysid, status)
self._publish_velocity(sysid, status)
@@ -202,9 +202,9 @@ class VehicleStatusPublisher(Node):
logger.info(f"Created publisher: {topic_name}")
return self.fc_publishers[key]
- def _publish_position(self, sysid: int, status: mvv.ComponentStatus):
+ def _publish_position_gnss(self, sysid: int, status: mvv.ComponentStatus):
"""發布 GPS 位置"""
- if not self.rate_controller.should_publish(sysid, 'position'):
+ if not self.rate_controller.should_publish(sysid, 'position_gnss'):
return
pos = status.position
@@ -212,22 +212,27 @@ class VehicleStatusPublisher(Node):
return
publisher = self._get_or_create_publisher(
- sysid, 'position', sensor_msgs.msg.NavSatFix
+ sysid, 'position_gnss', fcmsg.GnssRaw
)
# 檢查是否有訂閱者
if publisher.get_subscription_count() == 0:
return
- msg = sensor_msgs.msg.NavSatFix()
- msg.latitude = pos.latitude
- msg.longitude = pos.longitude
- msg.altitude = pos.altitude if pos.altitude is not None else 0.0
+ msg = fcmsg.GnssRaw()
+ msg.stamp = self.get_clock().now().to_msg()
+ msg.latitude = float(pos.latitude)
+ msg.longitude = float(pos.longitude)
+ msg.altitude = float(pos.altitude) if pos.altitude is not None else 0.0
# GPS 狀態資訊
gps = status.gps
if gps.fix_type is not None:
- msg.status.status = gps.fix_type - 1 # MAVLink fix_type 轉 NavSatStatus
+ msg.fix_type = int(gps.fix_type)
+ if gps.satellites_visible is not None:
+ msg.satellites_visible = int(gps.satellites_visible)
+ msg.eph = float(gps.eph) / 100.0 if gps.eph is not None else float('nan')
+ msg.epv = float(gps.epv) / 100.0 if gps.epv is not None else float('nan')
publisher.publish(msg)
diff --git a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
index 6fd1914..af2ebfc 100644
--- a/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
+++ b/src/fc_network_adapter/tests/test_vehicleStatusPublisher.py
@@ -17,6 +17,7 @@ import std_msgs.msg
import sensor_msgs.msg
import geometry_msgs.msg
import mavros_msgs.msg
+import fc_interfaces.msg
# 專案 imports
from ..fc_network_adapter.mavlinkROS2Nodes import (
@@ -39,7 +40,7 @@ class TestSubscriber(Node):
self.sysid = sysid
self.received_messages = {
- 'position': [],
+ 'position_gnss': [],
'attitude': [],
'velocity': [],
'battery': [],
@@ -60,9 +61,9 @@ class TestSubscriber(Node):
# Position
self.create_subscription(
- sensor_msgs.msg.NavSatFix,
- f'{base_topic}/position',
- lambda msg: self._on_message('position', msg),
+ fc_interfaces.msg.GnssRaw,
+ f'{base_topic}/position_gnss',
+ lambda msg: self._on_message('position_gnss', msg),
10
)
@@ -121,7 +122,7 @@ class TestSubscriber(Node):
def _format_msg(self, topic_name: str, msg) -> str:
"""格式化消息以便顯示"""
- if topic_name == 'position':
+ if topic_name == 'position_gnss':
return f"lat={msg.latitude:.6f}, lon={msg.longitude:.6f}, alt={msg.altitude:.2f}m"
elif topic_name == 'attitude':
return f"quat=({msg.orientation.x:.3f}, {msg.orientation.y:.3f}, {msg.orientation.z:.3f}, {msg.orientation.w:.3f})"
@@ -264,7 +265,7 @@ def test_basic_publishing():
# 檢查收到的消息
print("\n--- 消息統計 ---")
total_messages = 0
- for topic in ['position', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
+ for topic in ['position_gnss', 'attitude', 'velocity', 'battery', 'vfr_hud', 'mode', 'summary']:
count = test_sub.get_message_count(topic)
total_messages += count
print(f" {topic:15s}: {count:3d} 條消息")
@@ -304,7 +305,7 @@ def test_frequency_control():
# 修改頻率設定
publisher_node = ros2_manager.status_publisher
publisher_node.rate_controller.topic_intervals = {
- 'position': 1.5,
+ 'position_gnss': 1.5,
'attitude': 1.0,
'velocity': 1.0,
'battery': 1.0,
@@ -329,7 +330,7 @@ def test_frequency_control():
print("預期：position 約 2 條 (0.67Hz)，attitude/battery/velocity 約 3 條 (1Hz)，vfr_hud 約 6 條 (2Hz) mode/summary 不發布")
print("2Hz Topics (預期 ~6 條):")
- for topic in ['position', 'attitude', 'velocity', 'vfr_hud']:
+ for topic in ['position_gnss', 'attitude', 'velocity', 'vfr_hud']:
count = test_sub.get_message_count(topic)
print(f" {topic:15s}: {count:3d} 條")
for topic in ['battery', 'mode', 'summary']: