2026.6.24实现 QGC_WAYPOINTS → 系统内部航点格式 的转换链路

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

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 暂不处理

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)
+
     # ================================================================
     #  🔌 启动
     # ================================================================

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