|
|
#!/usr/bin/env python3
|
|
|
"""
|
|
|
指令發送模組
|
|
|
負責將目標位置轉成具體的通訊指令發送到飛控
|
|
|
|
|
|
現階段: MavlinkSender (直接 pymavlink 發送)
|
|
|
未來: 替換為 ROS2Sender (發到 ROS2 topic，由 fc_network_adapter 轉發)
|
|
|
"""
|
|
|
from abc import ABC, abstractmethod
|
|
|
from pymavlink import mavutil
|
|
|
|
|
|
|
|
|
class CommandSender(ABC):
|
|
|
"""指令發送抽象介面"""
|
|
|
|
|
|
@abstractmethod
|
|
|
def send_position_global(self, sysid, lat, lon, alt):
|
|
|
"""
|
|
|
發送全球座標位置指令
|
|
|
|
|
|
Args:
|
|
|
sysid: 目標無人機的 MAVLink system ID
|
|
|
lat: 緯度 (度)
|
|
|
lon: 經度 (度)
|
|
|
alt: 高度 (公尺)
|
|
|
"""
|
|
|
pass
|
|
|
|
|
|
@abstractmethod
|
|
|
def close(self):
|
|
|
"""關閉連線"""
|
|
|
pass
|
|
|
|
|
|
|
|
|
class MavlinkSender(CommandSender):
|
|
|
"""
|
|
|
MAVLink 直接發送 (驗證階段用)
|
|
|
透過 SET_POSITION_TARGET_GLOBAL_INT 發送目標位置
|
|
|
|
|
|
使用方式:
|
|
|
sender = MavlinkSender("udpout:127.0.0.1:14550")
|
|
|
sender.send_position_global(sysid=1, lat=24.123, lon=120.567, alt=10.0)
|
|
|
"""
|
|
|
|
|
|
# type_mask: 只使用位置 (lat, lon, alt)
|
|
|
# 忽略速度 (bit 3,4,5)、加速度 (bit 6,7,8)、yaw (bit 10)、yaw_rate (bit 11)
|
|
|
TYPE_MASK_POSITION_ONLY = (
|
|
|
0b0000_1101_1111_1000 # = 0x0DF8
|
|
|
)
|
|
|
|
|
|
def __init__(self, connection_string="udpout:127.0.0.1:14550"):
|
|
|
"""
|
|
|
Args:
|
|
|
connection_string: MAVLink 連線字串
|
|
|
SITL 範例: "udpout:127.0.0.1:14550"
|
|
|
"""
|
|
|
self.connection_string = connection_string
|
|
|
self.mav = mavutil.mavlink_connection(connection_string)
|
|
|
print(f"MavlinkSender 已建立連線: {connection_string}")
|
|
|
|
|
|
def send_position_global(self, sysid, lat, lon, alt):
|
|
|
"""
|
|
|
發送 SET_POSITION_TARGET_GLOBAL_INT
|
|
|
|
|
|
注意:
|
|
|
- coordinate_frame 使用 MAV_FRAME_GLOBAL_RELATIVE_ALT_INT
|
|
|
高度是相對起飛點的高度 (公尺)
|
|
|
- 如果 FormationPlanner 產出的 alt 是 AMSL 絕對高度，
|
|
|
需要在外部先減去起飛點高度再傳入
|
|
|
"""
|
|
|
self.mav.mav.set_position_target_global_int_send(
|
|
|
0, # time_boot_ms (不使用)
|
|
|
sysid, # target_system
|
|
|
1, # target_component (autopilot)
|
|
|
mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT_INT,
|
|
|
self.TYPE_MASK_POSITION_ONLY,
|
|
|
int(lat * 1e7), # lat_int
|
|
|
int(lon * 1e7), # lon_int
|
|
|
float(alt), # alt
|
|
|
0, 0, 0, # vx, vy, vz (忽略)
|
|
|
0, 0, 0, # afx, afy, afz (忽略)
|
|
|
0, 0 # yaw, yaw_rate (忽略)
|
|
|
)
|
|
|
|
|
|
def close(self):
|
|
|
"""關閉 MAVLink 連線"""
|
|
|
if self.mav:
|
|
|
self.mav.close()
|
|
|
self.mav = None
|
|
|
print("MavlinkSender 已關閉") |