From 84f9e6b2ee263c4252f9b814c29b1e2306e4478f Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Fri, 12 Jun 2026 09:31:52 +0800 Subject: [PATCH] =?UTF-8?q?=E9=80=9A=E8=AE=AF=E9=87=8D=E6=9E=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- drone_controller/router_proxy.py | 406 +++++++++++++++++++++++++++++++ infer_camera_modular.py | 397 +++++++++++++++++++++++++----- 2 files changed, 736 insertions(+), 67 deletions(-) create mode 100644 drone_controller/router_proxy.py diff --git a/drone_controller/router_proxy.py b/drone_controller/router_proxy.py new file mode 100644 index 0000000..3073ee0 --- /dev/null +++ b/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 已关闭") diff --git a/infer_camera_modular.py b/infer_camera_modular.py index 0b23c26..29a1db1 100644 --- a/infer_camera_modular.py +++ b/infer_camera_modular.py @@ -7,12 +7,24 @@ import queue import socket import json import sys -from ais_bench.infer.interface import InferSession -# ================== 导入控制模块 ===================== -from drone_controller.base_control import DroneController +# ================== 昇腾 NPU SDK（保护性导入） ===================== +# 说明：本机可能无昇腾环境，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") - baud_rate = fc_cfg.get("baud_rate", 57600) + # ---- 保存 config（供 start_uav 使用如 takeoff_alt 等） ---- + self.fc_cfg = fc_cfg + # ----------------------------------------------------------- - # 初始化控制驱动端 - self.uav = DroneController(connection_string=connection_string, baud=baud_rate) + # ---- 使用 RouterProxy 替换 DroneController ---- + 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( - 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"] - ) - self.pid_z = PID( - 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.pid_y = PID(**pid_y_cfg) + self.pid_z = PID(**pid_z_cfg) + + # 控制参数（按协议改为 20Hz，§4.1 要求 ≥20Hz） + self.control_hz = 20.0 + self.control_interval = 1.0 / self.control_hz + + # 线程控制 + 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): - """ 连接并解锁无人机 """ - 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() + """连接 Router，执行标准起飞序列（协议 §3.3.1） - def process_control(self, detections, orig_shape, frame): - """ 根据检测结果执行 MAVLink 闭环控制逻辑 """ - img_center_x = orig_shape[1] / 2 - img_center_y = orig_shape[0] / 2 - current_mode = getattr(self.uav, 'current_mode', 'UNKNOWN') + 流程： + ① 启动 ZMQ 代理（开始收 STATE） + ② 等待 Router 状态推送 + ③ 如需解锁 → ARM + ④ 如需起飞 → WAYPOINT(TAKEOFF) + ⑤ SETPOINT 预热（悬停）→ COMMAND(OFFBOARD) + ⑥ 启动 20Hz 控制线程 + """ + # ---- ① 启动 ZMQ 代理 ---- + self.proxy.start() + print("🔄 RouterProxy 已启动，等待 Router 状态推送...") - if len(detections) > 0: - # 寻找置信度最高的目标 - best_det = max(detections, key=lambda x: x[4]) - x1, y1, x2, y2, conf, cls_id = best_det - target_x = (x1 + x2) / 2 - target_y = (y1 + y2) / 2 + # ---- ② 等待收到第一条 STATE ---- + state = None + for i in range(50): # 最多等 5 秒 + state = self.proxy.get_latest_state() + if state is not None: + print("✅ 收到 Router 状态推送") + break + time.sleep(0.1) - # 计算归一化误差 - err_x = (target_x - img_center_x) / img_center_x - err_y = (target_y - img_center_y) / img_center_y + if state is None: + print("❌ 无法获取飞机状态，请检查 Router 是否运行，" + "且 SUB 端口 (tcp://127.0.0.1:5556) 可连通") + return False - # PID 计算转化为物理速度需求 - vy_cmd = self.pid_y.update(err_x) # 图像X正向 -> 飞机Vy右移 - vz_cmd = self.pid_z.update(err_y) # 图像Y正向 -> 飞机Vz下移 (NED系) - vx_cmd = 0.0 # 保持前后平移为0 + armed = state["drone"]["armed"] + mode = state["drone"]["mode"] + alt_rel = state["drone"]["alt_rel"] + age_us, _ = self.proxy.get_state_freshness(state) + age_s = (age_us or 0) / 1_000_000 - # 视效叠加 - cv2.line(frame, (int(img_center_x), int(img_center_y)), (int(target_x), int(target_y)), (255, 0, 0), 2) - 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) + print(f"📊 初始状态: mode={mode} | armed={armed} | " + f"alt_rel={alt_rel:.1f}m | state_age={age_s:.2f}s") - # 执行机体速度控制 - if current_mode in ['GUIDED', 'OFFBOARD']: - self.uav.send_body_velocity(vx=vx_cmd, vy=vy_cmd, vz=vz_cmd, yaw_rate=0.0) + # ---- ③ 解锁（如未解锁） ---- + if not armed: + if self._do_arm(): + armed = True else: - cv2.putText(frame, f"MANUAL MODE OVERRIDE ({current_mode})", (20, 100), cv2.FONT_HERSHEY_SIMPLEX, 0.6, - (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) + return False - # 清除 PID 积分，防止突跳 - if hasattr(self.pid_y, 'reset'): self.pid_y.reset() - if hasattr(self.pid_z, 'reset'): self.pid_z.reset() + # ---- ④ 起飞到目标高度（如高度不足） ---- + takeoff_alt = self.fc_cfg.get("takeoff_alt", 15.0) - if current_mode in ['GUIDED', 'OFFBOARD']: - self.uav.send_body_velocity(vx=0.0, vy=0.0, vz=0.0, yaw_rate=0.0) + if alt_rel < takeoff_alt * 0.9: + 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"]) - # 启动无人机连接 - controller.start_uav() + # 启动无人机连接（RouterProxy 启动 + 起飞序列） + if not controller.start_uav(): + print("❌ 飞控启动失败，退出") + sys.exit(1) - - print("🚀 NPU 精准推理 [JSON配置动态加载版] 主循环启动...") + print("🚀 NPU 精准推理 [ZMQ Router 协议版] 主循环启动...") + 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), - 2) + cv2.putText(frame, f"NPU Pure FPS: {fps_pure:.1f}", (20, 30), + 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() \ No newline at end of file + main()