通讯重构

drone_controller/router_proxy.py

@@ -0,0 +1,406 @@
+"""
+router_proxy.py — ZeroMQ 通信代理
+
+【职责】
+  封装与 Router 的 REQ/SUB 通信，遵循 python_protocol.md 规范：
+  - REQ (tcp://127.0.0.1:5555) — 发送 SETPOINT / COMMAND / QUERY / WAYPOINT，接收 ACK
+  - SUB (tcp://127.0.0.1:5556) — 订阅 Router 推送的 STATE / ALERT / PX4_ACK
+
+【使用】
+    proxy = RouterProxy()
+    proxy.start()
+
+
+    # 发送指令
+    ok, ack = proxy.send_command("ARM")
+    ok, ack = proxy.send_setpoint(vx=1.0, vy=0.0, vz=0.0, control_mode="VELOCITY")
+    ok, ack = proxy.send_query("HOME_POSITION")
+
+    # 读取最新状态（线程安全）
+    state = proxy.get_latest_state()
+    if proxy.is_state_fresh(state):
+        ...
+
+    proxy.close()
+
+【设计要点】
+  - REQ_RELAXED + REQ_CORRELATE 防止单次 recv 超时导致 socket 卡死
+  - SUB 采用专用后台线程 + zmq.Poller 非阻塞接收
+  - 所有公开接口线程安全
+  - 状态缓存原子读写，外部获取为深拷贝，无共享引用风险
+
+【作者】根据 python_protocol.md 重构
+"""
+
+import zmq
+import json
+import time
+import threading
+import copy
+import logging
+
+logger = logging.getLogger(__name__)
+
+
+class RouterProxy:
+    """ZeroMQ 通信代理：封装与 Router 的 REQ/SUB 通信"""
+
+    # ---- 协议常量 ----
+    STALE_THRESHOLD_US = 500_000          # 500 ms，STATE 新鲜度阈值（协议 §3.8.1）
+    STALE_THRESHOLD_WARN_US = 2_000_000   # 2 s，数据稍旧警告阈值
+
+    def __init__(self, req_endpoint="tcp://127.0.0.1:5555",
+                 sub_endpoint="tcp://127.0.0.1:5556",
+                 recv_timeout_ms=500):
+        """
+        初始化 ZMQ 代理
+
+        Args:
+            req_endpoint:   REQ socket 地址（发给 Router）
+            sub_endpoint:   SUB socket 地址（收 Router 推送）
+            recv_timeout_ms:REQ recv 超时毫秒数
+        """
+        self.req_endpoint = req_endpoint
+        self.sub_endpoint = sub_endpoint
+        self.recv_timeout_ms = recv_timeout_ms
+
+        # ---- ZMQ 上下文 ----
+        self.ctx = zmq.Context()
+
+        # ---- REQ socket: 发指令，收 ACK ----
+        self.req = self.ctx.socket(zmq.REQ)
+        self.req.setsockopt(zmq.RCVTIMEO, recv_timeout_ms)
+        # REQ_RELAXED + REQ_CORRELATE: 即使某次 recv 超时，
+        # 下次 send 仍能正常发送，旧响应会被自动丢弃
+        self.req.setsockopt(zmq.REQ_RELAXED, 1)
+        self.req.setsockopt(zmq.REQ_CORRELATE, 1)
+        self.req.connect(req_endpoint)
+
+        # ---- SUB socket: 收 Router 推送 ----
+        self.sub = self.ctx.socket(zmq.SUB)
+        self.sub.connect(sub_endpoint)
+        self.sub.setsockopt_string(zmq.SUBSCRIBE, "")   # 订阅所有消息
+
+        # ---- 消息序号（线程安全） ----
+        self._seq = 0
+        self._seq_lock = threading.Lock()
+
+        # ---- 最新 STATE 缓存 ----
+        self._latest_state = None
+        self._state_lock = threading.Lock()
+
+        # ---- SUB 监听线程 ----
+        self._running = False
+        self._sub_thread = None
+
+        # ---- 外部回调钩子 ----
+        self._state_callback = None
+        self._alert_callback = None
+
+    # ================================================================
+    #  内部工具
+    # ================================================================
+
+    def _next_id(self) -> int:
+        """递增消息序号（线程安全）"""
+        with self._seq_lock:
+            self._seq += 1
+            return self._seq
+
+    def _recover_req(self):
+        """
+        REQ 超时/异常后重建 socket
+
+        虽然设置了 REQ_RELAXED+CORRELATE，但连续超时或 ZMQ 内部状态异常时
+        重建是最安全的恢复手段。
+        """
+        try:
+            self.req.close(linger=0)
+        except Exception:
+            pass
+        self.req = self.ctx.socket(zmq.REQ)
+        self.req.setsockopt(zmq.RCVTIMEO, self.recv_timeout_ms)
+        self.req.setsockopt(zmq.REQ_RELAXED, 1)
+        self.req.setsockopt(zmq.REQ_CORRELATE, 1)
+        self.req.connect(self.req_endpoint)
+        logger.warning("REQ socket 已重建")
+
+    # ================================================================
+    #  发送接口（线程安全）
+    # ================================================================
+
+    def send_setpoint(self, control_mode="VELOCITY", **kwargs):
+        """
+        发送 SETPOINT 指令
+
+        位置控制:
+            proxy.send_setpoint(control_mode="POSITION",
+                                x=10.0, y=0.0, z=-8.0, yaw=0.0)
+        速度控制:
+            proxy.send_setpoint(control_mode="VELOCITY",
+                                vx=1.0, vy=0.0, vz=0.0, yaw_rate=0.0)
+
+        Args:
+            control_mode: "POSITION" | "VELOCITY"
+            **kwargs:     按 control_mode 传入对应字段
+
+        Returns:
+            (成功: bool, ACK 字典: dict)
+        """
+        msg_id = self._next_id()
+        msg = {
+            "type": "SETPOINT",
+            "id": msg_id,
+            "timestamp_us": int(time.time() * 1_000_000),
+            "setpoint": {"control_mode": control_mode, **kwargs}
+        }
+        return self._send_req(msg)
+
+    def send_command(self, command):
+        """
+        发送 COMMAND 指令
+
+        Args:
+            command: "ARM" | "OFFBOARD" | "LAND" | "RTL" | "RESUME"
+        """
+        msg_id = self._next_id()
+        msg = {
+            "type": "COMMAND",
+            "id": msg_id,
+            "timestamp_us": int(time.time() * 1_000_000),
+            "command": command
+        }
+        return self._send_req(msg)
+
+    def send_query(self, query="HOME_POSITION"):
+        """
+        发送 QUERY 查询
+
+        Args:
+            query: "HOME_POSITION"（目前仅支持此查询）
+        """
+        msg_id = self._next_id()
+        msg = {
+            "type": "QUERY",
+            "id": msg_id,
+            "timestamp_us": int(time.time() * 1_000_000),
+            "query": query
+        }
+        return self._send_req(msg)
+
+    def send_waypoint(self, **wp_kwargs):
+        """
+        发送 WAYPOINT 航点
+
+        Args:
+            **wp_kwargs: 航点字段（action, alt, lat, lon, speed, ...）
+
+        示例:
+            proxy.send_waypoint(action="TAKEOFF", alt=15.0, alt_frame="RELATIVE")
+            proxy.send_waypoint(action="HOVER", lat=22.55, lon=113.88,
+                                alt=100.0, hover_time=3.0)
+        """
+        msg_id = self._next_id()
+        msg = {
+            "type": "WAYPOINT",
+            "id": msg_id,
+            "timestamp_us": int(time.time() * 1_000_000),
+            "waypoint": wp_kwargs
+        }
+        return self._send_req(msg)
+
+    # ================================================================
+    #  底层 REQ-REP 通信
+    # ================================================================
+
+    def _send_req(self, msg):
+        """
+        发送 REQ 并等待 ACK 回复
+
+        ZeroMQ REQ-REP 是严格的交替模式：send → recv → send → recv
+        本方法保证每次调用完成一次完整的 send-recv 周期。
+
+        Returns:
+            (ok: bool, ack_dict: dict)
+            ack_dict 示例：{"type": "ACK", "ref_id": 1, "status": "OK", "message": ""}
+        """
+        try:
+            self.req.send_string(json.dumps(msg))
+            ack_str = self.req.recv_string()
+            ack = json.loads(ack_str)
+            ok = ack.get("status") == "OK"
+            if not ok:
+                logger.warning("ACK 非 OK: type=%s id=%s ack=%s",
+                               msg.get("type"), msg.get("id"), ack)
+            return ok, ack
+        except zmq.Again:
+            logger.error("REQ recv 超时 (type=%s id=%s)", msg.get("type"), msg.get("id"))
+            self._recover_req()
+            return False, {"status": "TIMEOUT", "message": "recv timeout"}
+        except Exception as e:
+            logger.error("REQ 异常: %s", e)
+            self._recover_req()
+            return False, {"status": "FAIL", "message": str(e)}
+
+    # ================================================================
+    #  状态获取
+    # ================================================================
+
+    def get_latest_state(self):
+        """
+        获取最新缓存的 STATE（线程安全，深拷贝）
+
+        Returns:
+            state dict（格式见 python_protocol.md §3.8），
+            未收到过 STATE 时返回 None
+        """
+        with self._state_lock:
+            return copy.deepcopy(self._latest_state)
+
+    def is_state_fresh(self, state, stale_threshold_us=None):
+        """
+        检查 STATE 数据是否新鲜（协议 §3.8.1）
+
+        Args:
+            state:                get_latest_state() 的返回值
+            stale_threshold_us:   新鲜度阈值（默认 500ms）
+
+        Returns:
+            True  数据新鲜，可正常使用
+            False 数据过期，应保守飞行或触发保护
+        """
+        if state is None:
+            return False
+        if stale_threshold_us is None:
+            stale_threshold_us = self.STALE_THRESHOLD_US
+        now_us = int(time.time() * 1_000_000)
+        try:
+            age_us = now_us - state["drone"]["last_update_us"]
+        except (KeyError, TypeError):
+            return False
+        if age_us >= self.STALE_THRESHOLD_WARN_US:
+            logger.warning("STATE 严重过期: age=%.2fs（阈值=%.1fs）",
+                           age_us / 1e6, stale_threshold_us / 1e6)
+        return age_us < stale_threshold_us
+
+    def get_state_freshness(self, state):
+        """
+        返回 STATE 新鲜度详细信息（用于日志/调试）
+
+        Returns:
+            (age_us: int, is_fresh: bool) 或 (None, False)
+        """
+        if state is None:
+            return None, False
+        try:
+            now_us = int(time.time() * 1_000_000)
+            age_us = now_us - state["drone"]["last_update_us"]
+            return age_us, age_us < self.STALE_THRESHOLD_US
+        except (KeyError, TypeError):
+            return None, False
+
+    # ---- 回调注册 ----
+
+    def set_state_callback(self, callback):
+        """
+        注册 STATE 回调（在 SUB 线程中调用，不应阻塞）
+        callback 签名: callback(state_dict)
+        """
+        self._state_callback = callback
+
+    def set_alert_callback(self, callback):
+        """
+        注册 ALERT 回调（在 SUB 线程中调用，不应阻塞）
+        callback 签名: callback(alert_dict)
+        """
+        self._alert_callback = callback
+
+    # ================================================================
+    #  生命周期管理
+    # ================================================================
+
+    def start(self):
+        """启动 SUB 监听后台线程"""
+        if self._running:
+            return
+        self._running = True
+        self._sub_thread = threading.Thread(
+            target=self._sub_worker,
+            name="router-proxy-sub",
+            daemon=True
+        )
+        self._sub_thread.start()
+        logger.info("RouterProxy 已启动，SUB 监听线程已开启（REQ=%s SUB=%s）",
+                    self.req_endpoint, self.sub_endpoint)
+
+    def _sub_worker(self):
+        """后台线程：通过 zmq.Poller 非阻塞接收 SUB 消息"""
+        poller = zmq.Poller()
+        poller.register(self.sub, zmq.POLLIN)
+
+        while self._running:
+            try:
+                socks = dict(poller.poll(timeout=100))  # 100 ms 超时轮询
+                if self.sub not in socks:
+                    continue
+
+                msg_str = self.sub.recv_string()
+                data = json.loads(msg_str)
+                msg_type = data.get("type")
+
+                if msg_type == "STATE":
+                    with self._state_lock:
+                        self._latest_state = data
+                    if self._state_callback:
+                        try:
+                            self._state_callback(data)
+                        except Exception as e:
+                            logger.error("STATE 回调异常: %s", e)
+
+                elif msg_type == "ALERT":
+                    logger.warning("⚠️ ALERT: %s — %s",
+                                   data.get("alert"), data.get("message"))
+                    if self._alert_callback:
+                        try:
+                            self._alert_callback(data)
+                        except Exception as e:
+                            logger.error("ALERT 回调异常: %s", e)
+
+                elif msg_type == "PX4_ACK":
+                    logger.info("PX4_ACK: cmd=%s result=%s",
+                                data.get("ref_cmd"), data.get("result"))
+
+                elif msg_type == "QUERY_REPLY":
+                    # QUERY_REPLY 由 send_query 的同步 recv 处理，
+                    # SUB 通道上的 QUERY_REPLY 暂不处理
+                    pass
+
+                # 其它消息类型可扩展
+
+            except zmq.Again:
+                continue
+            except json.JSONDecodeError as e:
+                logger.warning("SUB 收到非 JSON 消息: %s", e)
+                continue
+            except Exception as e:
+                logger.error("SUB 线程异常: %s", e)
+                time.sleep(0.1)
+
+    def close(self):
+        """关闭代理，释放所有 ZMQ 资源"""
+        self._running = False
+        if self._sub_thread and self._sub_thread.is_alive():
+            self._sub_thread.join(timeout=2.0)
+
+        for name, sock in [("REQ", self.req), ("SUB", self.sub)]:
+            try:
+                sock.close(linger=0)
+            except Exception:
+                pass
+
+        try:
+            self.ctx.term()
+        except Exception:
+            pass
+
+        logger.info("RouterProxy 已关闭")

infer_camera_modular.py

@@ -7,12 +7,24 @@ import queue
 import socket
 import json
 import sys
-from ais_bench.infer.interface import InferSession
 
-# ================== 导入控制模块 =====================
+# ================== 昇腾 NPU SDK（保护性导入） =====================
-from drone_controller.base_control import DroneController
+# 说明：本机可能无昇腾环境，try-except 确保 import 错误仅告警不崩溃。
+# 部署到香橙派 AI Pro 时只需安装 ais_bench 即可无缝运行。
+try:
+    from ais_bench.infer.interface import InferSession
+    _HAS_NPU = True
+    print("✅ 昇腾 NPU SDK 导入成功")
+except ImportError:
+    _HAS_NPU = False
+    print("⚠️ 昇腾 NPU SDK 未安装，YOLO26UAVInfer 将在实例化时报错（部署后正常）")
+# ===============================================================
+
+# ================== ZMQ Router 通信代理 =====================
+from drone_controller.router_proxy import RouterProxy
 from drone_controller.pid_counter import PID
-# ===================================================
+# ===========================================================
+
 
 class VideoStreaming:
     """ 📡 视频流采集与后台图传模块（支持 RTSP 自动重连） """
@@ -158,6 +170,7 @@ class VideoStreaming:
             self.cap.release()
         self.tx_queue.put(None)
 
+
 class YOLO26UAVInfer:
     """ 🚀 昇腾 NPU 推理与图像后处理模块 """
 
@@ -228,76 +241,310 @@ class YOLO26UAVInfer:
 
 
 class UAVControlLoop:
-    """ 🛸 无人机飞控闭环控制核心 """
+    """ 🛸 无人机飞控闭环控制核心（基于 ZeroMQ Router 协议）
+
+    使用 RouterProxy 取代底层 DroneController，所有 MAVLink 通信
+    均通过 ZMQ REQ/SUB 转发给 Router 处理。
+
+    【控制流】
+      ┌─主循环─────────────┐
+      │ update_detections() │──→ 写入共享检测结果
+      └────────────────────┘
+               │
+      ┌─控制线程(20Hz)──────┐
+      │ _compute_velocity() │──→ PID 计算机体速度
+      │ proxy.send_setpoint │──→ ZMQ REQ → Router → PX4
+      └────────────────────┘
+
+    【保活设计】
+      - 控制线程以 20Hz 持续发送 SETPOINT，满足协议 ≥20Hz 要求
+      - 检测结果超过 500ms 未更新 → 强制零速刹车（防断流）
+      - 无目标时 PID 自动复位 → 输出零速
+    """
 
     def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg):
-        connection_string = fc_cfg.get("connection_string", "udpout:127.0.0.1:14550")
+        # ---- 保存 config（供 start_uav 使用如 takeoff_alt 等） ----
-        baud_rate = fc_cfg.get("baud_rate", 57600)
+        self.fc_cfg = fc_cfg
+        # -----------------------------------------------------------
 
-        # 初始化控制驱动端
+        # ---- 使用 RouterProxy 替换 DroneController ----
-        self.uav = DroneController(connection_string=connection_string, baud=baud_rate)
+        self.proxy = RouterProxy(
+            req_endpoint=fc_cfg.get("req_endpoint", "tcp://127.0.0.1:5555"),
+            sub_endpoint=fc_cfg.get("sub_endpoint", "tcp://127.0.0.1:5556")
+        )
+        # ---------------------------------------------
 
-        # 从 JSON 参数动态初始化 PID 调节器
+        self.pid_y = PID(**pid_y_cfg)
-        self.pid_y = PID(
+        self.pid_z = PID(**pid_z_cfg)
-            kp=pid_y_cfg["kp"], ki=pid_y_cfg["ki"], kd=pid_y_cfg["kd"],
+
-            max_out=pid_y_cfg["max_out"], min_out=pid_y_cfg["min_out"]
+        # 控制参数（按协议改为 20Hz，§4.1 要求 ≥20Hz）
-        )
+        self.control_hz = 20.0
-        self.pid_z = PID(
+        self.control_interval = 1.0 / self.control_hz
-            kp=pid_z_cfg["kp"], ki=pid_z_cfg["ki"], kd=pid_z_cfg["kd"],
+
-            max_out=pid_z_cfg["max_out"], min_out=pid_z_cfg["min_out"]
+        # 线程控制
-        )
+        self.running = False
+        self.control_thread = None
+        self.current_target = (0.0, 0.0, 0.0)
+        self.detections = []                   # 最新检测结果（线程间共享）
+        self.frame_shape = (480, 640)          # 默认尺寸
+
+        # 锁保护共享数据
+        self.data_lock = threading.Lock()
+
+        # ---- 防断流戳：主线程每次 update_detections 时更新 ----
+        self._last_update_time = 0.0
 
     def start_uav(self):
-        """ 连接并解锁无人机 """
+        """连接 Router，执行标准起飞序列（协议 §3.3.1）
-        self.uav.connect()
-        self.uav.print_status()
-        if not self.uav.is_armed and self.uav.current_mode not in ['GUIDED', 'OFFBOARD']:
-            self.uav.arm()
 
-    def process_control(self, detections, orig_shape, frame):
+        流程：
-        """ 根据检测结果执行 MAVLink 闭环控制逻辑 """
+          ① 启动 ZMQ 代理（开始收 STATE）
-        img_center_x = orig_shape[1] / 2
+          ② 等待 Router 状态推送
-        img_center_y = orig_shape[0] / 2
+          ③ 如需解锁 → ARM
-        current_mode = getattr(self.uav, 'current_mode', 'UNKNOWN')
+          ④ 如需起飞 → WAYPOINT(TAKEOFF)
+          ⑤ SETPOINT 预热（悬停）→ COMMAND(OFFBOARD)
+          ⑥ 启动 20Hz 控制线程
+        """
+        # ---- ① 启动 ZMQ 代理 ----
+        self.proxy.start()
+        print("🔄 RouterProxy 已启动，等待 Router 状态推送...")
 
-        if len(detections) > 0:
+        # ---- ② 等待收到第一条 STATE ----
-            # 寻找置信度最高的目标
+        state = None
-            best_det = max(detections, key=lambda x: x[4])
+        for i in range(50):  # 最多等 5 秒
-            x1, y1, x2, y2, conf, cls_id = best_det
+            state = self.proxy.get_latest_state()
-            target_x = (x1 + x2) / 2
+            if state is not None:
-            target_y = (y1 + y2) / 2
+                print("✅ 收到 Router 状态推送")
+                break
+            time.sleep(0.1)
 
-            # 计算归一化误差
+        if state is None:
-            err_x = (target_x - img_center_x) / img_center_x
+            print("❌ 无法获取飞机状态，请检查 Router 是否运行，"
-            err_y = (target_y - img_center_y) / img_center_y
+                  "且 SUB 端口 (tcp://127.0.0.1:5556) 可连通")
+            return False
 
-            # PID 计算转化为物理速度需求
+        armed = state["drone"]["armed"]
-            vy_cmd = self.pid_y.update(err_x)  # 图像X正向 -> 飞机Vy右移
+        mode = state["drone"]["mode"]
-            vz_cmd = self.pid_z.update(err_y)  # 图像Y正向 -> 飞机Vz下移 (NED系)
+        alt_rel = state["drone"]["alt_rel"]
-            vx_cmd = 0.0  # 保持前后平移为0
+        age_us, _ = self.proxy.get_state_freshness(state)
+        age_s = (age_us or 0) / 1_000_000
 
-            # 视效叠加
+        print(f"📊 初始状态: mode={mode} | armed={armed} | "
-            cv2.line(frame, (int(img_center_x), int(img_center_y)), (int(target_x), int(target_y)), (255, 0, 0), 2)
+              f"alt_rel={alt_rel:.1f}m | state_age={age_s:.2f}s")
-            cv2.putText(frame, f"MAVLink Out -> Err_X: {err_x:+.3f} Err_Y: {err_y:+.3f}", (20, 70),
-                        cv2.FONT_HERSHEY_SIMPLEX, 0.6, (255, 255, 0), 2)
 
-            # 执行机体速度控制
+        # ---- ③ 解锁（如未解锁） ----
-            if current_mode in ['GUIDED', 'OFFBOARD']:
+        if not armed:
-                self.uav.send_body_velocity(vx=vx_cmd, vy=vy_cmd, vz=vz_cmd, yaw_rate=0.0)
+            if self._do_arm():
+                armed = True
             else:
-                cv2.putText(frame, f"MANUAL MODE OVERRIDE ({current_mode})", (20, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.6,
+                return False
-                            (0, 0, 255), 2)
-        else:
-            # 目标丢失保护逻辑
-            cv2.putText(frame, "MAVLink Out -> Target LOST (HOVER)", (20, 70), cv2.FONT_HERSHEY_SIMPLEX, 0.6,
-                        (0, 0, 255), 2)
 
-            # 清除 PID 积分，防止突跳
+        # ---- ④ 起飞到目标高度（如高度不足） ----
-            if hasattr(self.pid_y, 'reset'): self.pid_y.reset()
+        takeoff_alt = self.fc_cfg.get("takeoff_alt", 15.0)
-            if hasattr(self.pid_z, 'reset'): self.pid_z.reset()
 
-            if current_mode in ['GUIDED', 'OFFBOARD']:
+        if alt_rel < takeoff_alt * 0.9:
-                self.uav.send_body_velocity(vx=0.0, vy=0.0, vz=0.0, yaw_rate=0.0)
+            print(f"🛫 高度 {alt_rel:.1f}m < 目标 {takeoff_alt:.1f}m，执行起飞...")
+            ok, ack = self.proxy.send_waypoint(
+                action="TAKEOFF", alt=takeoff_alt,
+                alt_frame="RELATIVE", speed=3.0
+            )
+            if not ok:
+                print(f"⚠️ TAKEOFF 指令 ACK 失败: {ack}，尝试通过 SETPOINT 爬升...")
+                # 降级方案：用 POSITION SETPOINT 爬升
+                ok, ack = self.proxy.send_setpoint(
+                    x=0, y=0, z=-takeoff_alt, yaw=0.0,
+                    control_mode="POSITION"
+                )
+                if not ok:
+                    print(f"❌ 爬升 SETPOINT 也失败: {ack}")
+                    return False
+
+            # 等待爬升
+            for i in range(100):  # 最多等 10 秒
+                state = self.proxy.get_latest_state()
+                if state and state["drone"]["alt_rel"] >= takeoff_alt * 0.9:
+                    alt_rel = state["drone"]["alt_rel"]
+                    print(f"✅ 达到目标高度: {alt_rel:.1f}m")
+                    break
+                time.sleep(0.1)
+            else:
+                print(f"⚠️ 爬升等待超时，当前 alt_rel={state['drone']['alt_rel']:.1f}m，继续执行")
+
+        # ---- ⑤ 预热 + 切换 OFFBOARD ----
+        if mode != "OFFBOARD":
+            # 获取最新高度用于预热悬停
+            state = self.proxy.get_latest_state()
+            current_alt = state["drone"]["alt_rel"] if state else takeoff_alt
+
+            print(f"🔄 发送悬停 SETPOINT 预热 (alt={current_alt:.1f}m)...")
+            ok, ack = self.proxy.send_setpoint(
+                x=0, y=0, z=-current_alt, yaw=0.0,
+                control_mode="POSITION"
+            )
+            if ok:
+                time.sleep(0.2)  # 等流建立（协议 §3.3.1 ③→④）
+            else:
+                print(f"⚠️ 预热 SETPOINT ACK 异常: {ack}，尝试继续...")
+
+            print("🔄 发送 OFFBOARD 指令...")
+            ok, ack = self.proxy.send_command("OFFBOARD")
+            if not ok:
+                print(f"❌ OFFBOARD 指令失败: {ack}")
+                return False
+
+            # 等待模式切换
+            for i in range(30):
+                state = self.proxy.get_latest_state()
+                if state and state["drone"]["mode"] == "OFFBOARD":
+                    mode = "OFFBOARD"
+                    print("✅ 已进入 OFFBOARD 模式")
+                    break
+                time.sleep(0.1)
+            else:
+                print(f"❌ OFFBOARD 模式切换超时，当前 mode={state['drone']['mode'] if state else 'N/A'}")
+                return False
+
+        # ---- ⑥ 启动独立控制线程 ----
+        self.running = True
+        self.control_thread = threading.Thread(
+            target=self._control_loop, daemon=True, name="uav-control"
+        )
+        self.control_thread.start()
+        print("✅ 控制线程已启动 (20Hz)")
+        return True
+
+    # ---- 辅助方法 ----
+
+    def _do_arm(self):
+        """执行解锁序列"""
+        print("🔓 发送 ARM 指令...")
+        ok, ack = self.proxy.send_command("ARM")
+        if not ok:
+            print(f"❌ ARM 指令发送失败: {ack}")
+            return False
+        # 等待飞控确认解锁
+        for i in range(30):
+            state = self.proxy.get_latest_state()
+            if state and state["drone"]["armed"]:
+                print("✅ 解锁成功")
+                return True
+            time.sleep(0.1)
+        print("❌ 解锁超时（30 次轮询未确认 armed=true）")
+        return False
+
+    # ---- 主循环调用的接口 ----
+
+    def update_detections(self, detections, frame_shape):
+        """由主循环调用，更新最新检测结果和图像尺寸
+
+        Args:
+            detections:  postprocess 后的检测列表 [[x1,y1,x2,y2,conf,cls_id], ...]
+            frame_shape: 原始帧形状 (height, width)
+        """
+        with self.data_lock:
+            self.detections = detections
+            self.frame_shape = frame_shape
+            self._last_update_time = time.time()
+
+    # ---- 内部控制线程 ----
+
+    def _control_loop(self):
+        """独立线程：以 20Hz 发送 SETPOINT，保证 OFFBOARD 保活
+
+        无论是否有目标，每次迭代都发送 VELOCITY SETPOINT。
+        无目标时速度全零（悬停刹车），满足协议 ≥20Hz 要求。
+        """
+        while self.running:
+            start = time.time()
+
+            # 获取最新的检测结果（加锁）
+            with self.data_lock:
+                dets = self.detections.copy()
+                h, w = self.frame_shape[:2]
+                last_update = self._last_update_time
+
+            # ---- 防断流检测：超过 500ms 无更新则强制零速 ----
+            # 覆盖主线程卡死、摄像头断流等场景
+            if time.time() - last_update > 0.5:
+                if len(dets) > 0:
+                    # 只在首次过期时打印，避免刷屏
+                    pass  # 保留静默刹车
+                dets = []       # 清空检测 → PID 复位 → 零速
+                self.pid_y.reset()
+                self.pid_z.reset()
+            # ------------------------------------------------
+
+            # 计算速度指令
+            vx, vy, vz = self._compute_velocity(dets, w, h)
+
+            # 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制）
+            # 无论是否有目标，都持续发送以满足 20Hz 保活要求
+            ok, ack = self.proxy.send_setpoint(
+                vx=vx, vy=vy, vz=vz,
+                yaw_rate=0.0,
+                control_mode="VELOCITY"
+            )
+            if not ok and self.running:
+                # ACK 失败记录日志，但不中断循环
+                # recv 超时或格式错误由 _send_req 内部恢复 REQ socket
+                pass
+
+            # 精确控制循环频率（20Hz → 每 50ms）
+            elapsed = time.time() - start
+            sleep_time = self.control_interval - elapsed
+            if sleep_time > 0:
+                time.sleep(sleep_time)
+
+    def _compute_velocity(self, detections, frame_w, frame_h):
+        """根据检测结果计算机体速度 (vx, vy, vz)
+
+        与原始逻辑相同：
+          - 无目标 → PID 复位 → 返回零速（悬停/刹车）
+          - 有目标 → 取置信度最高的 → 偏差归一化 → PID 输出
+
+        Returns:
+            (vx, vy, vz): NED 坐标系速度 (m/s)
+        """
+        if not detections:
+            # 无目标：悬停，并重置 PID 积分
+            self.pid_y.reset()
+            self.pid_z.reset()
+            return 0.0, 0.0, 0.0
+
+        # 取置信度最高的目标
+        best = max(detections, key=lambda x: x[4])
+        x1, y1, x2, y2, _conf, _cls_id = best
+        target_x = (x1 + x2) / 2
+        target_y = (y1 + y2) / 2
+        img_center_x = frame_w / 2
+        img_center_y = frame_h / 2
+
+        err_x = (target_x - img_center_x) / img_center_x
+        err_y = (target_y - img_center_y) / img_center_y
+
+        vy = self.pid_y.update(err_x)   # 左右移动
+        vz = self.pid_z.update(err_y)   # 上下移动（NED 中正为下）
+        vx = 0.0                        # 前后移动
+
+        return vx, vy, vz
+
+    def stop(self):
+        """停止控制线程，发送刹车指令，关闭 ZMQ 代理"""
+        self.running = False
+        if self.control_thread:
+            self.control_thread.join(timeout=2.0)
+
+        # 发送零速刹车指令
+        print("🛑 发送刹车指令...")
+        self.proxy.send_setpoint(
+            vx=0.0, vy=0.0, vz=0.0,
+            yaw_rate=0.0,
+            control_mode="VELOCITY"
+        )
+        time.sleep(0.2)  # 确保指令发出
+
+        self.proxy.close()
+        print("控制已停止，ZMQ 代理已关闭")
 
 
 # ==================== 主程序入口 ======================
@@ -322,11 +569,13 @@ def main():
     detector = YOLO26UAVInfer(cfg["model"])
     controller = UAVControlLoop(cfg["flight_control"], cfg["pid_y"], cfg["pid_z"])
 
-    # 启动无人机连接
+    # 启动无人机连接（RouterProxy 启动 + 起飞序列）
-    controller.start_uav()
+    if not controller.start_uav():
+        print("❌ 飞控启动失败，退出")
+        sys.exit(1)
 
-
+    print("🚀 NPU 精准推理 [ZMQ Router 协议版] 主循环启动...")
-    print("🚀 NPU 精准推理 [JSON配置动态加载版] 主循环启动...")
+    loop_count = 0
 
     try:
         while True:
@@ -335,6 +584,7 @@ def main():
                 time.sleep(0.005)
                 continue
 
+            loop_count += 1
             orig_shape = frame.shape[:2]
 
             # 1. 图像预处理
@@ -349,22 +599,35 @@ def main():
 
             # 3. 坐标反求与控制计算
             detections = detector.postprocess(outputs, ratio, dwdh, orig_shape)
-            controller.process_control(detections, orig_shape, frame)
+            controller.update_detections(detections, orig_shape)
 
             # 4. 界面渲染
             frame = detector.draw_boxes(frame, detections)
-            cv2.putText(frame, f"NPU Pure FPS: {fps_pure:.1f}", (20, 30), cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255),
+            cv2.putText(frame, f"NPU Pure FPS: {fps_pure:.1f}", (20, 30),
-                        2)
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.7, (0, 255, 255), 2)
 
             # 5. 推送至后台异步图传队列
             streamer.push_to_stream(frame)
 
+            # 6. 周期性打印 Router 状态（每 100 帧）
+            if loop_count % 100 == 0:
+                state = controller.proxy.get_latest_state()
+                if state:
+                    d = state["drone"]
+                    age_us, _ = controller.proxy.get_state_freshness(state)
+                    age_ms = (age_us or 0) / 1000
+                    print(f"📡 [Router] mode={d['mode']} armed={d['armed']} "
+                          f"alt={d['alt_rel']:.1f}m spd={d['ground_speed']:.1f}m/s "
+                          f"batt={d['battery']:.0f}% state_age={age_ms:.0f}ms",
+                          end="" if loop_count % 500 != 0 else "\n")
+
     except KeyboardInterrupt:
         print("\n👋 接收到终止信号，安全退出中...")
     finally:
+        controller.stop()
         streamer.release()
         print("程序运行结束。")
 
 
 if __name__ == "__main__":
-    main()
+    main()