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