From 25882a5f8b3998f8a0185eb8543a6c2ace23a318 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Wed, 24 Jun 2026 12:50:31 +0800 Subject: [PATCH] =?UTF-8?q?2026.6.24=E5=AE=9E=E7=8E=B0=20QGC=5FWAYPOINTS?= =?UTF-8?q?=20=E2=86=92=20=E7=B3=BB=E7=BB=9F=E5=86=85=E9=83=A8=E8=88=AA?= =?UTF-8?q?=E7=82=B9=E6=A0=BC=E5=BC=8F=20=E7=9A=84=E8=BD=AC=E6=8D=A2?= =?UTF-8?q?=E9=93=BE=E8=B7=AF?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- drone_controller/mission_manager.py | 11 ++- drone_controller/router_proxy.py | 16 +++++ run_mission.py | 106 ++++++++++++++++++++++++++-- utils/coord.py | 31 ++++++++ 4 files changed, 157 insertions(+), 7 deletions(-) create mode 100644 utils/coord.py diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py index 1adde4d..d43ae4c 100644 --- a/drone_controller/mission_manager.py +++ b/drone_controller/mission_manager.py @@ -163,7 +163,7 @@ class MissionManager: # UAVControlLoop 或 run_mission.py 在判定视觉追踪完成时置 True self.tracking_completed = False # 追踪最大时长兜底 - self.tracking_max_duration = 30.0 + self.tracking_max_duration = 10.0 logger.info("MissionManager 初始化: %d 个航点, 目标高度=%.1fm, " "到达半径=%.1fm, 搜寻时长=%.1fs, 返航=%s", @@ -712,3 +712,12 @@ class MissionManager: "z": wp.get("z", self.target_altitude), "yaw": wp.get("yaw", 0), } + + # mission_manager.py 中新增 + + def set_waypoints(self, waypoints): + """由外部调用，动态注入/更新航点列表""" + self.waypoints = list(waypoints) + self.current_wp_index = 0 + self._waypoints_ready = True # 这个标志位结合 WAITING_WAYPOINTS 状态使用 + logger.info("MissionManager 航点已更新，共 %d 个", len(waypoints)) diff --git a/drone_controller/router_proxy.py b/drone_controller/router_proxy.py index a68aa2f..f3920ba 100644 --- a/drone_controller/router_proxy.py +++ b/drone_controller/router_proxy.py @@ -103,6 +103,7 @@ class RouterProxy: # ---- 外部回调钩子 ---- self._state_callback = None self._alert_callback = None + self._waypoints_callback = None # ================================================================ # 内部工具 @@ -155,6 +156,13 @@ class RouterProxy: self.req.connect(self.req_endpoint) logger.warning("REQ socket 已重建") + def set_waypoints_callback(self, callback): + """ + 注册 QGC_WAYPOINT 回调 （在 SUB 线程调用，不应阻塞） + callback 签名：callback(waypoint_data: dict) + """ + self._waypoint_callback = callback + # ================================================================ # 发送接口（线程安全） # ================================================================ @@ -432,6 +440,14 @@ class RouterProxy: logger.info("PX4_ACK: cmd=%s result=%s", data.get("ref_cmd"), data.get("result")) + elif msg_type == "QGC_WAYPOINTS": + logger.info("📥 收到 QGC_WAYPOINTS: count=%d", data.get("count", 0)) + if self._waypoints_callback: + try: + self._waypoints_callback(data) + except Exception as e: + logger.error("QGC_WAYPOINTS 回调异常: %s", e) + elif msg_type == "QUERY_REPLY": # QUERY_REPLY 由 send_query 的同步 recv 处理， # SUB 通道上的 QUERY_REPLY 暂不处理 diff --git a/run_mission.py b/run_mission.py index 5f0e51e..fc4dc9c 100644 --- a/run_mission.py +++ b/run_mission.py @@ -35,27 +35,27 @@ run_mission.py — 基于 ZMQ Router 协议的主任务入口 即可，保持 read_frame() → (ret, frame) 的协议不变。 """ -import os -import sys -import time import json import logging +import os +import sys import threading +import time import cv2 -# ---- ZMQ Router 代理与状态机 ---- -from drone_controller.router_proxy import RouterProxy from drone_controller.mission_manager import ( MissionManager, MissionState, FailsafeTriggered, ) - +# ---- ZMQ Router 代理与状态机 ---- +from drone_controller.router_proxy import RouterProxy # ---- 视觉闭环控制（复用 infer_camera_modular 现有实现） ---- # 注意：此处不修改 UAVControlLoop 任何内部逻辑。 # run_mission.py 在外部协调其控制线程的启停。 from infer_camera_modular import UAVControlLoop, VideoStreaming, YOLO26UAVInfer +from utils.coord import latlon_to_ned logger = logging.getLogger("RunMission") @@ -97,6 +97,8 @@ class MissionOrchestrator: sub_endpoint=fc.get("sub_endpoint", "tcp://127.0.0.1:5556"), ) + self.mission_proxy.set_waypoints_callback(self._on_qgc_waypoints) + self.mission = MissionManager( proxy=self.mission_proxy, waypoints=self.waypoints, @@ -122,6 +124,98 @@ class MissionOrchestrator: self.logger.info("MissionOrchestrator 初始化完成") + # ================================================================ + # 📥 QGC 航点回调（由 RouterProxy 的 SUB 线程触发） + # ================================================================ + + def _on_qgc_waypoints(self,data: dict): + """ + 收到 QGC 下发的航点列表 -> 转换坐标系 -> 注入 MissionManager + """ + try: + raw_wps = data.get("waypoints", []) + if not raw_wps: + self.logger.warning("QGC 航点列表为空，请检查是否提交或路由问题") + return + + self.logger.info("📥 收到 %d 个航点，开始坐标转换...", len(raw_wps)) + + # ---- ① 获取参考点（起飞点） ---- + state = self.mission_proxy.get_latest_state() + if state is None: + self.logger.error("无法获取 STATE，缺少参考经纬度，航点转换失败") + return + + home = state.get("home", {}) + ref_lat = home.get("lat", 0.0) + ref_lon = home.get("lon", 0.0) + ref_alt = home.get("alt", 0.0) + + # ---- 容错：如果 HOME 未初始化，用第一个航点作为参考（纯相对飞行） ---- + if ref_lat == 0.0 and ref_lon == 0.0: + self.logger.warning("STATE.home 未初始化，使用第一个航点作为参考原点") + ref_lat = raw_wps[0].get("lat", 0.0) + ref_lon = raw_wps[0].get("lon", 0.0) + ref_alt = raw_wps[0].get("z", 0.0) + + self.logger.info("参考原点 (起飞点): lat=%.6f, lon=%.6f, alt=%.1fm", + ref_lat, ref_lon, ref_alt) + + # ---- ② 逐点转换 ---- + converted = [] + for idx, wp in enumerate(raw_wps): + lat = wp.get("lat", 0.0) + lon = wp.get("lon", 0.0) + alt = wp.get("z", 0.0) # QGC 中 z 为海拔 + cmd = wp.get("command", 16) # 16=WAYPOINT, 22=TAKEOFF, 21=LAND + yaw = wp.get("param4", 0.0) # param4 通常存偏航角 + hold_time = wp.get("param1", 8.0) # 悬停时间（如有） + + x, y, z = latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt) + + # 根据 MAV_CMD 类型构建内部航点 + if cmd == 22: # TAKEOFF + # 起飞点强制归零水平位置，高度取绝对海拔差 + converted.append({ + "x": 0.0, + "y": 0.0, + "z": z, # 已按 NED 转换（向上为负） + "yaw": yaw, + "command": "TAKEOFF", + "hold_duration": hold_time, + }) + self.logger.info("航点 %d [TAKEOFF]: 高度 %.1fm", idx, -z) + + elif cmd == 21: # LAND + converted.append({ + "x": x, + "y": y, + "z": 0.0, # 着陆点 z=0（地面） + "yaw": yaw, + "command": "LAND", + }) + self.logger.info("航点 %d [LAND]: NED(%.1f, %.1f)", idx, x, y) + + else: # 普通航点 (command=16 或未识别) + converted.append({ + "x": x, + "y": y, + "z": z, # NED 高度 + "yaw": yaw, + "command": "WAYPOINT", + "hold_duration": hold_time, + }) + self.logger.debug("航点 %d [WAYPOINT]: NED(%.1f, %.1f, %.1f)", + idx, x, y, z) + + self.mission.set_waypoints(converted) + + # 如果当前处于等待航点状态，状态机会在下一帧自动响应 + self.logger.info("✅ 航点转换完成，共 %d 个航点已加载", len(converted)) + + except Exception as e: + self.logger.error("航点处理异常: %s",e) + # ================================================================ # 🔌 启动 # ================================================================ diff --git a/utils/coord.py b/utils/coord.py new file mode 100644 index 0000000..db17d64 --- /dev/null +++ b/utils/coord.py @@ -0,0 +1,31 @@ +import math + +def latlon_to_ned(lat, lon, alt, ref_lat, ref_lon, ref_alt): + """ + 将 WGS-84 经纬高 (LLA) 转换为局部 NED 坐标 (x, y, z) + + Args: + lat, lon, alt: 目标点的纬度(deg)、经度(deg)、海拔(m) + ref_lat, ref_lon, ref_alt: 参考点（起飞点）的纬度、经度、海拔 + + Returns: + (x, y, z): NED 坐标 (米) + x: 北向距离 (正北为正) + y: 东向距离 (正东为正) + z: 向下距离 (向下为正，即向上为负) + """ + # WGS-84 椭球参数（赤道半径，单位：米） + EARTH_RADIUS = 6378137.0 + # 1. 纬度差 -> 北向距离 (x) + dlat = math.radians(lat - ref_lat) + x = dlat * EARTH_RADIUS + + # 2. 经度差 -> 东向距离 (y)，需乘以纬度的余弦值修正 + dlon = math.radians(lon - ref_lon) + y = dlon * EARTH_RADIUS * math.cos(math.radians(ref_lat)) + + # 3. 高度差 -> NED Z (向下为正) + # 若目标海拔高于参考点，则 z 为负（向上） + z = -(alt - ref_alt) + + return x, y, z \ No newline at end of file