199 lines
9.5 KiB
Python
199 lines
9.5 KiB
Python
import threading
|
|
import time
|
|
from pymavlink import mavutil
|
|
|
|
|
|
class QGCMissionReceiver:
|
|
def __init__(self, connection_str='udpin:0.0.0.0:14550', sys_id=1, comp_id=1, home_lat=34.0, home_lon=113.0):
|
|
"""
|
|
QGC 航线拦截接收器 (模拟飞控节点)
|
|
:param connection_str: pymavlink连接字符串。udpin:0.0.0.0:14550 表示作为本地服务器等待QGC主动连接
|
|
:param sys_id: 伪装的系统ID (通常飞控为1)
|
|
:param comp_id: 伪装的组件ID (通常飞控为1)
|
|
:param home_lat: 模拟的起始点纬度（用于在没有真实GPS的室内环境让QGC地图定点）
|
|
:param home_lon: 模拟的起始点经度
|
|
"""
|
|
self.connection_str = connection_str
|
|
self.sys_id = sys_id
|
|
self.comp_id = comp_id
|
|
self.home_lat = home_lat
|
|
self.home_lon = home_lon
|
|
|
|
# 初始化 MAVLink 链路
|
|
self.master = mavutil.mavlink_connection(
|
|
self.connection_str,
|
|
source_system=self.sys_id,
|
|
source_component=self.comp_id
|
|
)
|
|
|
|
# 线程安全与航点存储
|
|
self.lock = threading.Lock()
|
|
self.waypoints_list = [] # 正式供外部控制脚本读取的航点列表
|
|
self._temp_waypoints = [] # 协议传输过程中的临时缓存缓存
|
|
|
|
# 协议状态控制变量
|
|
self.is_running = False
|
|
self.expected_count = 0
|
|
self.current_seq = 0
|
|
|
|
def start(self):
|
|
"""启动后台模拟线程"""
|
|
self.is_running = True
|
|
|
|
# 线程 1-A: 维持心跳与状态遥测广播 (5Hz)
|
|
self.telemetry_thread = threading.Thread(target=self._telemetry_loop, daemon=True)
|
|
self.telemetry_thread.start()
|
|
|
|
# 线程 1-B: 专门阻塞监听并应答 QGC 的航线握手协议
|
|
self.protocol_thread = threading.Thread(target=self._protocol_loop, daemon=True)
|
|
self.protocol_thread.start()
|
|
|
|
print(f"[QGCMissionProxy] 虚拟飞控线程已启动，正在监听 UDP 端口: {self.connection_str}")
|
|
|
|
def stop(self):
|
|
"""停止接收器"""
|
|
self.is_running = False
|
|
if self.master:
|
|
self.master.close()
|
|
print("[QGCMissionProxy] 虚拟飞控线程已关闭")
|
|
|
|
def _telemetry_loop(self):
|
|
"""高频向QGC广播心跳和定位，激活QGC的航线规划和上传按钮"""
|
|
boot_time = time.time()
|
|
while self.is_running:
|
|
try:
|
|
time_ms = int((time.time() - boot_time) * 1000)
|
|
|
|
# 1. 发送 HEARTBEAT (伪装成运行中的 ArduPilot 多旋翼，且处于 GUIDED 模式解锁状态)
|
|
self.master.mav.heartbeat_send(
|
|
mavutil.mavlink.MAV_TYPE_QUADROTOR, # 四轴/多旋翼
|
|
mavutil.mavlink.MAV_AUTOPILOT_ARDUPILOT, # 固件类型
|
|
mavutil.mavlink.MAV_MODE_FLAG_GUIDED_ENABLED | mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED,
|
|
0, # custom_mode
|
|
mavutil.mavlink.MAV_STATE_ACTIVE # 活动状态
|
|
)
|
|
|
|
# 2. 发送 GLOBAL_POSITION_INT (不发这个QGC地图无法显示飞机，也无法规划航线)
|
|
self.master.mav.global_position_int_send(
|
|
time_ms,
|
|
int(self.home_lat * 1e7), # 纬度 (degrees * 1E7)
|
|
int(self.home_lon * 1e7), # 经度 (degrees * 1E7)
|
|
10000, # 绝对高度 (毫米, 10米)
|
|
0, # 相对高度 (毫米, 0米)
|
|
0, 0, 0, # vx, vy, vz 速度 (cm/s)
|
|
9000 # hdg 偏航角 (cdeg, 90度朝东)
|
|
)
|
|
|
|
# 3. 发送系统状态 (维持QGC右上角图标正常显示，可选)
|
|
self.master.mav.sys_status_send(0, 0, 0, 500, 11100, 100, 80, 0, 0, 0, 0, 0, 0)
|
|
|
|
except Exception as e:
|
|
print(f"[QGCMissionProxy] 遥测发送异常: {e}")
|
|
time.sleep(0.2) # 5Hz 频率广播
|
|
|
|
def _protocol_loop(self):
|
|
"""标准 MAVLink 航线协议状态机逻辑"""
|
|
while self.is_running:
|
|
try:
|
|
# 阻塞读取消息
|
|
msg = self.master.recv_match(blocking=True, timeout=0.5)
|
|
if not msg:
|
|
continue
|
|
|
|
msg_type = msg.get_type()
|
|
src_sys = msg.get_srcSystem()
|
|
src_comp = msg.get_srcComponent()
|
|
|
|
# ---- 阶段 A: QGC点击了“发送航线”，通知飞机即将上传的总数 ----
|
|
if msg_type == 'MISSION_COUNT':
|
|
self.expected_count = msg.count
|
|
self.current_seq = 0
|
|
self._temp_waypoints = []
|
|
print(f"\n[QGCMissionProxy] 拦截到QGC航线上传请求！预计航点总数: {self.expected_count}")
|
|
|
|
if self.expected_count > 0:
|
|
# 核心回复：主动向地面站索要第 0 个航点数据
|
|
self.master.mav.mission_request_int_send(
|
|
src_sys, src_comp, self.current_seq, msg.mission_type
|
|
)
|
|
else:
|
|
# 总数为0则直接发送成功确认
|
|
self.master.mav.mission_ack_send(
|
|
src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, msg.mission_type
|
|
)
|
|
|
|
# ---- 阶段 B: 接收具体的航点细节 (新版QGC通常使用 MISSION_ITEM_INT 整型坐标协议) ----
|
|
elif msg_type == 'MISSION_ITEM_INT':
|
|
if msg.seq == self.current_seq:
|
|
# 解析QGC发来的单条数据
|
|
wp = {
|
|
'seq': msg.seq,
|
|
'frame': msg.frame,
|
|
'command': msg.command,
|
|
'lat': msg.x / 1e7, # 还原浮点经纬度
|
|
'lon': msg.y / 1e7,
|
|
'alt': msg.z, # 高度
|
|
'param1': msg.param1, # 悬停时间等控制参数
|
|
'param2': msg.param2,
|
|
'param3': msg.param3,
|
|
'param4': msg.param4
|
|
}
|
|
self._temp_waypoints.append(wp)
|
|
print(
|
|
f"[QGCMissionProxy] 成功接收并缓存航点 [{msg.seq + 1}/{self.expected_count}]: Lat={wp['lat']:.6f}, Lon={wp['lon']:.6f}, Alt={wp['alt']:.2f}m")
|
|
|
|
self.current_seq += 1
|
|
if self.current_seq < self.expected_count:
|
|
# 继续索要下一个航点
|
|
self.master.mav.mission_request_int_send(
|
|
src_sys, src_comp, self.current_seq, msg.mission_type
|
|
)
|
|
else:
|
|
# ---- 阶段 C: 全部接收完毕，原子化存入正式列表，并向QGC回复“握手成功” ----
|
|
with self.lock:
|
|
self.waypoints_list = list(self._temp_waypoints)
|
|
print(f"🎉 [QGCMissionProxy] 航线完美拦截并填充！列表当前长度: {len(self.waypoints_list)}")
|
|
|
|
self.master.mav.mission_ack_send(
|
|
src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED, msg.mission_type
|
|
)
|
|
|
|
# ---- 兼容机制: 兼容老版本QGC使用的浮点数协议形式 ----
|
|
elif msg_type == 'MISSION_ITEM':
|
|
if msg.seq == self.current_seq:
|
|
wp = {
|
|
'seq': msg.seq, 'frame': msg.frame, 'command': msg.command,
|
|
'lat': msg.x, 'lon': msg.y, 'alt': msg.z,
|
|
'param1': msg.param1, 'param2': msg.param2, 'param3': msg.param3, 'param4': msg.param4
|
|
}
|
|
self._temp_waypoints.append(wp)
|
|
self.current_seq += 1
|
|
if self.current_seq < self.expected_count:
|
|
self.master.mav.mission_request_send(src_sys, src_comp, self.current_seq, msg.mission_type)
|
|
else:
|
|
with self.lock:
|
|
self.waypoints_list = list(self._temp_waypoints)
|
|
self.master.mav.mission_ack_send(src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED,
|
|
msg.mission_type)
|
|
|
|
# ---- 处理地面站点击“清除航线”的请求 ----
|
|
elif msg_type == 'MISSION_CLEAR_ALL':
|
|
print("[QGCMissionProxy] 拦截到QGC清除航线指令")
|
|
with self.lock:
|
|
self.waypoints_list = []
|
|
self.master.mav.mission_ack_send(src_sys, src_comp, mavutil.mavlink.MAV_MISSION_ACCEPTED,
|
|
msg.mission_type)
|
|
|
|
except Exception as e:
|
|
print(f"[QGCMissionProxy] 协议状态机运行异常: {e}")
|
|
time.sleep(0.1)
|
|
|
|
def get_waypoints(self):
|
|
"""提供给控制主线程调用的线程安全方法：获取当前最新的航点列表"""
|
|
with self.lock:
|
|
return list(self.waypoints_list)
|
|
|
|
def clear_waypoints(self):
|
|
"""从香橙派端手动清空列表"""
|
|
with self.lock:
|
|
self.waypoints_list = [] |