songhaoyu/opi-yolo
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

通讯重构

Browse Source
This commit is contained in:
songhaoyu
2026-06-12 09:31:52 +08:00
parent eadbae2337
commit 84f9e6b2ee
2 changed files with 736 additions and 67 deletions
Download Patch File Download Diff File Expand all files Collapse all files
drone_controller/router_proxy.py
+406
View File
@@ -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
+326 -63
View File
@@ -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
# ---- ② 等待收到第一条 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)
if state is None:
print("❌ 无法获取飞机状态，请检查 Router 是否运行，"
"且 SUB 端口 (tcp://127.0.0.1:5556) 可连通")
return False
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
print(f"📊 初始状态: mode={mode} | armed={armed} | "
f"alt_rel={alt_rel:.1f}m | state_age={age_s:.2f}s")
# ---- ③ 解锁（如未解锁） ----
if not armed:
if self._do_arm():
armed = True
else:
return False
# ---- ④ 起飞到目标高度（如高度不足） ----
takeoff_alt = self.fc_cfg.get("takeoff_alt", 15.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
# 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
vy = self.pid_y.update(err_x) # 左右移动
vz = self.pid_z.update(err_y) # 上下移动（NED 中正为下）
vx = 0.0 # 前后移动
# 视效叠加
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)
return vx, vy, vz
# 执行机体速度控制
if current_mode in ['GUIDED', 'OFFBOARD']:
self.uav.send_body_velocity(vx=vx_cmd, vy=vy_cmd, vz=vz_cmd, yaw_rate=0.0)
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)
def stop(self):
"""停止控制线程，发送刹车指令，关闭 ZMQ 代理"""
self.running = False
if self.control_thread:
self.control_thread.join(timeout=2.0)
# 清除 PID 积分，防止突跳
if hasattr(self.pid_y, 'reset'): self.pid_y.reset()
if hasattr(self.pid_z, 'reset'): self.pid_z.reset()
# 发送零速刹车指令
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) # 确保指令发出
if current_mode in ['GUIDED', 'OFFBOARD']:
self.uav.send_body_velocity(vx=0.0, vy=0.0, vz=0.0, yaw_rate=0.0)
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,19 +599,32 @@ 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("程序运行结束。")