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 15:19:47 +08:00
parent 3c5388f256
commit 651907ce3d
2 changed files with 179 additions and 57 deletions
Download Patch File Download Diff File Expand all files Collapse all files
drone_controller/router_proxy.py
+55 -7
View File
@@ -89,6 +89,9 @@ class RouterProxy:
self._latest_state = None self._latest_state = None
self._state_lock = threading.Lock() self._state_lock = threading.Lock()
# ---- 首次 STATE 标记 ----
self._first_state_received = False
# ---- SUB 监听线程 ---- # ---- SUB 监听线程 ----
self._running = False self._running = False
self._sub_thread = None self._sub_thread = None
@@ -107,6 +110,21 @@ class RouterProxy:
self._seq += 1 self._seq += 1
return self._seq return self._seq
def _build_send_summary(self, msg: dict) -> str:
"""构建发送消息的内容概要，用于日志"""
msg_type = msg.get("type", "?")
if msg_type == "SETPOINT":
sp = msg.get("setpoint", {})
return ", ".join(f"{k}={v}" for k, v in sp.items())
elif msg_type == "COMMAND":
return f"command={msg.get('command')}"
elif msg_type == "QUERY":
return f"query={msg.get('query')}"
elif msg_type == "WAYPOINT":
wp = msg.get("waypoint", {})
return ", ".join(f"{k}={v}" for k, v in wp.items())
return str(msg)
def _recover_req(self): def _recover_req(self):
""" """
REQ 超时/异常后重建 socket REQ 超时/异常后重建 socket
@@ -224,21 +242,27 @@ class RouterProxy:
(ok: bool, ack_dict: dict) (ok: bool, ack_dict: dict)
ack_dict 示例：{"type": "ACK", "ref_id": 1, "status": "OK", "message": ""} ack_dict 示例：{"type": "ACK", "ref_id": 1, "status": "OK", "message": ""}
""" """
msg_type = msg.get("type", "?")
msg_id = msg.get("id", "?")
summary = self._build_send_summary(msg)
logger.debug("Sending %s id=%s: %s", msg_type, msg_id, summary)
try: try:
self.req.send_string(json.dumps(msg)) self.req.send_string(json.dumps(msg))
ack_str = self.req.recv_string() ack_str = self.req.recv_string()
ack = json.loads(ack_str) ack = json.loads(ack_str)
ok = ack.get("status") == "OK" ok = ack.get("status") == "OK"
logger.info("ACK received for id=%s, status=%s",
ack.get("ref_id"), ack.get("status"))
if not ok: if not ok:
logger.warning("ACK 非 OK: type=%s id=%s ack=%s", logger.warning("ACK 非 OK: type=%s id=%s ack=%s",
msg.get("type"), msg.get("id"), ack) msg_type, msg_id, ack)
return ok, ack return ok, ack
except zmq.Again: except zmq.Again:
logger.error("REQ recv 超时 (type=%s id=%s)", msg.get("type"), msg.get("id")) logger.error("REQ recv 超时 (type=%s id=%s)", msg_type, msg_id)
self._recover_req() self._recover_req()
return False, {"status": "TIMEOUT", "message": "recv timeout"} return False, {"status": "TIMEOUT", "message": "recv timeout"}
except Exception as e: except Exception as e:
logger.error("REQ 异常: %s", e) logger.error("REQ 异常 (type=%s id=%s): %s", msg_type, msg_id, e)
self._recover_req() self._recover_req()
return False, {"status": "FAIL", "message": str(e)} return False, {"status": "FAIL", "message": str(e)}
@@ -278,9 +302,13 @@ class RouterProxy:
age_us = now_us - state["drone"]["last_update_us"] age_us = now_us - state["drone"]["last_update_us"]
except (KeyError, TypeError): except (KeyError, TypeError):
return False return False
if age_us >= self.STALE_THRESHOLD_WARN_US: if age_us >= stale_threshold_us:
logger.warning("STATE 严重过期: age=%.2fs（阈值=%.1fs）", if age_us >= self.STALE_THRESHOLD_WARN_US:
age_us / 1e6, stale_threshold_us / 1e6) logger.warning("STATE 严重过期: age=%.2fs（阈值=%.0fms）",
age_us / 1e6, stale_threshold_us / 1000)
else:
logger.warning("STATE 过期: age=%.1fms（阈值=%.0fms）",
age_us / 1000, stale_threshold_us / 1000)
return age_us < stale_threshold_us return age_us < stale_threshold_us
def get_state_freshness(self, state): def get_state_freshness(self, state):
@@ -351,6 +379,25 @@ class RouterProxy:
if msg_type == "STATE": if msg_type == "STATE":
with self._state_lock: with self._state_lock:
self._latest_state = data self._latest_state = data
if not self._first_state_received:
self._first_state_received = True
logger.info("首次收到 Router 状态推送")
# DEBUG 输出关键字段 + 新鲜度
try:
drone = data.get("drone", {})
now_us = int(time.time() * 1_000_000)
last_up = drone.get("last_update_us", now_us)
age_us = now_us - last_up
logger.debug("STATE: mode=%s armed=%s "
"alt_rel=%.1f batt=%.0f%% age=%.0fms",
drone.get("mode"), drone.get("armed"),
drone.get("alt_rel", 0),
drone.get("battery", 0),
age_us / 1000)
except Exception:
pass
if self._state_callback: if self._state_callback:
try: try:
self._state_callback(data) self._state_callback(data)
@@ -375,7 +422,8 @@ class RouterProxy:
# SUB 通道上的 QUERY_REPLY 暂不处理 # SUB 通道上的 QUERY_REPLY 暂不处理
pass pass
# 其它消息类型可扩展 else:
logger.warning("收到未知消息类型: %s", msg_type)
except zmq.Again: except zmq.Again:
continue continue
infer_camera_modular.py
+124 -50
View File
@@ -7,6 +7,7 @@ import queue
import socket import socket
import json import json
import sys import sys
import logging
# ================== 昇腾 NPU SDK（保护性导入） ===================== # ================== 昇腾 NPU SDK（保护性导入） =====================
# 开发机可能无昇腾环境，try-except 确保 import 错误仅告警不崩溃。 # 开发机可能无昇腾环境，try-except 确保 import 错误仅告警不崩溃。
@@ -31,6 +32,7 @@ class VideoStreaming:
""" 📡 视频流采集与后台图传模块（支持 RTSP 自动重连） """ """ 📡 视频流采集与后台图传模块（支持 RTSP 自动重连） """
def __init__(self, video_cfg, net_cfg): def __init__(self, video_cfg, net_cfg):
self.logger = logging.getLogger("VideoStream")
self.video_source = video_cfg.get("video_source", 0) self.video_source = video_cfg.get("video_source", 0)
self.jpeg_quality = video_cfg.get("jpeg_quality", 75) self.jpeg_quality = video_cfg.get("jpeg_quality", 75)
self.gs_ip = net_cfg.get("ground_station_ip", "127.0.0.1") self.gs_ip = net_cfg.get("ground_station_ip", "127.0.0.1")
@@ -69,17 +71,21 @@ class VideoStreaming:
ret, frame = self.cap.read() ret, frame = self.cap.read()
if not ret: if not ret:
# 读取失败，关闭并标记需重连 # 读取失败，关闭并标记需重连
print("⚠️ RTSP 读取失败，准备重连...") self.logger.warning("RTSP 读取失败，准备重连...")
self.cap.release() self.cap.release()
self.cap = None self.cap = None
continue continue
# 清空旧帧，只保留最新一帧 # 清空旧帧，只保留最新一帧
dropped = 0
while not self.frame_buffer.empty(): while not self.frame_buffer.empty():
try: try:
self.frame_buffer.get_nowait() self.frame_buffer.get_nowait()
dropped += 1
except queue.Empty: except queue.Empty:
break break
if dropped > 0:
self.logger.debug("丢弃旧帧 %d 帧（缓冲区刷新）", dropped)
self.frame_buffer.put(frame) self.frame_buffer.put(frame)
if self.cap is not None: if self.cap is not None:
@@ -88,7 +94,7 @@ class VideoStreaming:
def _open_camera(self): def _open_camera(self):
""" 打开摄像头或 RTSP 流，并设置低延迟参数 """ """ 打开摄像头或 RTSP 流，并设置低延迟参数 """
try: try:
print(f"📷 正在连接视频源: {self.video_source}") self.logger.info("正在连接视频源: %s", self.video_source)
self.cap = cv2.VideoCapture(self.video_source) self.cap = cv2.VideoCapture(self.video_source)
if self.is_rtsp: if self.is_rtsp:
@@ -102,7 +108,7 @@ class VideoStreaming:
if "?" not in self.video_source: if "?" not in self.video_source:
self.video_source += "?tcp" self.video_source += "?tcp"
# 注意：有些相机需要重新初始化，这里简单打印提示 # 注意：有些相机需要重新初始化，这里简单打印提示
print("💡 建议使用 TCP 传输: 在 RTSP URL 后添加 ?tcp") self.logger.info("建议使用 TCP 传输: 在 RTSP URL 后添加 ?tcp")
else: else:
# USB 摄像头也可设置缓冲区 # USB 摄像头也可设置缓冲区
self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1) self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
@@ -112,11 +118,11 @@ class VideoStreaming:
# self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480) # self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
if self.cap.isOpened(): if self.cap.isOpened():
print("✅ 视频源连接成功") self.logger.info("视频源连接成功: %s", self.video_source)
else: else:
print("❌ 视频源打开失败") self.logger.error("视频源打开失败: %s", self.video_source)
except Exception as e: except Exception as e:
print(f"❌ 打开视频源异常: {e}") self.logger.error("打开视频源异常: %s", e)
self.cap = None self.cap = None
def read_frame(self): def read_frame(self):
@@ -137,10 +143,12 @@ class VideoStreaming:
pass pass
def _udp_stream_sender_worker(self): def _udp_stream_sender_worker(self):
""" 原图传发送线程（未改动） """ """ 原图传发送线程（增加帧统计日志） """
sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
server_address = (self.gs_ip, self.udp_port) server_address = (self.gs_ip, self.udp_port)
print(f"📡 图传后台线程已启动，目标地面站 -> {self.gs_ip}:{self.udp_port}") self.logger.info("图传后台线程已启动，目标地面站 -> %s:%s",
self.gs_ip, self.udp_port)
send_count = 0
while self.is_running: while self.is_running:
try: try:
@@ -153,12 +161,16 @@ class VideoStreaming:
continue continue
data = img_encode.tobytes() data = img_encode.tobytes()
if len(data) > 65000: if len(data) > 65000:
self.logger.debug("图像数据过大 (%d bytes), 跳过发送", len(data))
continue continue
sock.sendto(data, server_address) sock.sendto(data, server_address)
send_count += 1
if send_count % 100 == 0:
self.logger.info("图传统计: 已发送 %d 帧 UDP 数据包", send_count)
except queue.Empty: except queue.Empty:
continue continue
except Exception as e: except Exception as e:
print(f"❌ 图传线程异常: {e}") self.logger.exception("图传线程异常: %s", e)
time.sleep(0.1) time.sleep(0.1)
sock.close() sock.close()
@@ -176,6 +188,7 @@ class YOLO26UAVInfer:
""" 🚀 昇腾 NPU 推理与图像后处理模块 """ """ 🚀 昇腾 NPU 推理与图像后处理模块 """
def __init__(self, model_cfg): def __init__(self, model_cfg):
self.logger = logging.getLogger("YOLO")
model_path = model_cfg.get("model_path", "./om/yolo26n-balloon.om") model_path = model_cfg.get("model_path", "./om/yolo26n-balloon.om")
device_id = model_cfg.get("device_id", 0) device_id = model_cfg.get("device_id", 0)
self.conf_threshold = model_cfg.get("conf_threshold", 0.25) self.conf_threshold = model_cfg.get("conf_threshold", 0.25)
@@ -186,6 +199,8 @@ class YOLO26UAVInfer:
self.session = InferSession(device_id, model_path) self.session = InferSession(device_id, model_path)
self.input_width = 640 self.input_width = 640
self.input_height = 640 self.input_height = 640
self.logger.info("模型加载成功: path=%s input_size=%dx%d",
model_path, self.input_width, self.input_height)
def letterbox(self, img, new_shape=(640, 640), color=(114, 114, 114)): def letterbox(self, img, new_shape=(640, 640), color=(114, 114, 114)):
shape = img.shape[:2] # [height, width] shape = img.shape[:2] # [height, width]
@@ -208,6 +223,8 @@ class YOLO26UAVInfer:
input_tensor = np.transpose(input_tensor, (2, 0, 1)) input_tensor = np.transpose(input_tensor, (2, 0, 1))
input_tensor = np.expand_dims(input_tensor, axis=0) input_tensor = np.expand_dims(input_tensor, axis=0)
input_tensor = np.ascontiguousarray(input_tensor) input_tensor = np.ascontiguousarray(input_tensor)
self.logger.debug("预处理: 输入形状 %s -> %s",
str(frame.shape), str(input_tensor.shape))
return input_tensor, ratio, dwdh return input_tensor, ratio, dwdh
def postprocess(self, outputs, ratio, dwdh, orig_shape): def postprocess(self, outputs, ratio, dwdh, orig_shape):
@@ -229,6 +246,11 @@ class YOLO26UAVInfer:
x2 = max(0, min(x2, orig_shape[1])) x2 = max(0, min(x2, orig_shape[1]))
y2 = max(0, min(y2, orig_shape[0])) y2 = max(0, min(y2, orig_shape[0]))
detections.append([x1, y1, x2, y2, conf, cls_id]) detections.append([x1, y1, x2, y2, conf, cls_id])
if detections:
best = max(detections, key=lambda d: d[4])
self.logger.debug("检测到 %d 个目标, 最高置信度: %.3f 中心点: (%.0f, %.0f)",
len(detections), best[4],
(best[0] + best[2]) / 2, (best[1] + best[3]) / 2)
return detections return detections
def draw_boxes(self, img, detections): def draw_boxes(self, img, detections):
@@ -264,6 +286,8 @@ class UAVControlLoop:
""" """
def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg): def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg):
self.logger = logging.getLogger("UAVControlLoop")
# ---- 保存 config（供 start_uav 使用如 takeoff_alt 等） ---- # ---- 保存 config（供 start_uav 使用如 takeoff_alt 等） ----
self.fc_cfg = fc_cfg self.fc_cfg = fc_cfg
# ----------------------------------------------------------- # -----------------------------------------------------------
@@ -287,6 +311,8 @@ class UAVControlLoop:
min_hits=tracker_cfg.get("min_hits", 3), min_hits=tracker_cfg.get("min_hits", 3),
) )
self._tracked_result = None # 跟踪结果（锁保护） self._tracked_result = None # 跟踪结果（锁保护）
# 记录上一次跟踪状态，用于检测状态变化
self._last_tracked = False
# ------------------------------------------------ # ------------------------------------------------
# 控制参数（按协议改为 20Hz，§4.1 要求 ≥20Hz） # 控制参数（按协议改为 20Hz，§4.1 要求 ≥20Hz）
@@ -309,6 +335,11 @@ class UAVControlLoop:
# ---- 安全刹车请求标志（主线程判丢 → 控制线程执行刹车） ---- # ---- 安全刹车请求标志（主线程判丢 → 控制线程执行刹车） ----
self._brake_requested = False self._brake_requested = False
# ---- 最近一次发送的速度指令（用于周期性日志打印） ----
self._last_vx = 0.0
self._last_vy = 0.0
self._last_vz = 0.0
def start_uav(self): def start_uav(self):
"""连接 Router，执行标准起飞序列（协议 §3.3.1） """连接 Router，执行标准起飞序列（协议 §3.3.1）
@@ -322,20 +353,20 @@ class UAVControlLoop:
""" """
# ---- ① 启动 ZMQ 代理 ---- # ---- ① 启动 ZMQ 代理 ----
self.proxy.start() self.proxy.start()
print("🔄 RouterProxy 已启动，等待 Router 状态推送...") self.logger.info("RouterProxy 已启动，等待 Router 状态推送...")
# ---- ② 等待收到第一条 STATE ---- # ---- ② 等待收到第一条 STATE ----
state = None state = None
for i in range(50): # 最多等 5 秒 for i in range(50): # 最多等 5 秒
state = self.proxy.get_latest_state() state = self.proxy.get_latest_state()
if state is not None: if state is not None:
print("✅ 收到 Router 状态推送") self.logger.info("收到 Router 状态推送")
break break
time.sleep(0.1) time.sleep(0.1)
if state is None: if state is None:
print("❌ 无法获取飞机状态，请检查 Router 是否运行，" self.logger.error("无法获取飞机状态，请检查 Router 是否运行，"
"且 SUB 端口 (tcp://127.0.0.1:5556) 可连通") "且 SUB 端口 (tcp://127.0.0.1:5556) 可连通")
return False return False
armed = state["drone"]["armed"] armed = state["drone"]["armed"]
@@ -344,8 +375,8 @@ class UAVControlLoop:
age_us, _ = self.proxy.get_state_freshness(state) age_us, _ = self.proxy.get_state_freshness(state)
age_s = (age_us or 0) / 1_000_000 age_s = (age_us or 0) / 1_000_000
print(f"📊 初始状态: mode={mode} | armed={armed} | " self.logger.info("初始状态: mode=%s armed=%s alt_rel=%.1fm state_age=%.2fs",
f"alt_rel={alt_rel:.1f}m | state_age={age_s:.2f}s") mode, armed, alt_rel, age_s)
# ---- ③ 解锁（如未解锁） ---- # ---- ③ 解锁（如未解锁） ----
if not armed: if not armed:
@@ -358,20 +389,20 @@ class UAVControlLoop:
takeoff_alt = self.fc_cfg.get("takeoff_alt", 5.0) takeoff_alt = self.fc_cfg.get("takeoff_alt", 5.0)
if alt_rel < takeoff_alt * 0.9: if alt_rel < takeoff_alt * 0.9:
print(f"🛫 高度 {alt_rel:.1f}m < 目标 {takeoff_alt:.1f}m，执行起飞...") self.logger.info("高度 %.1fm < 目标 %.1fm，执行起飞...", alt_rel, takeoff_alt)
ok, ack = self.proxy.send_waypoint( ok, ack = self.proxy.send_waypoint(
action="TAKEOFF", alt=takeoff_alt, action="TAKEOFF", alt=takeoff_alt,
alt_frame="RELATIVE", speed=3.0 alt_frame="RELATIVE", speed=3.0
) )
if not ok: if not ok:
print(f"⚠️ TAKEOFF 指令 ACK 失败: {ack}，尝试通过 SETPOINT 爬升...") self.logger.warning("TAKEOFF 指令 ACK 失败: %s，尝试通过 SETPOINT 爬升...", ack)
# 降级方案：用 POSITION SETPOINT 爬升 # 降级方案：用 POSITION SETPOINT 爬升
ok, ack = self.proxy.send_setpoint( ok, ack = self.proxy.send_setpoint(
x=0, y=0, z=-takeoff_alt, yaw=0.0, x=0, y=0, z=-takeoff_alt, yaw=0.0,
control_mode="POSITION" control_mode="POSITION"
) )
if not ok: if not ok:
print(f"❌ 爬升 SETPOINT 也失败: {ack}") self.logger.error("爬升 SETPOINT 也失败: %s", ack)
return False return False
# 等待爬升 # 等待爬升
@@ -379,11 +410,12 @@ class UAVControlLoop:
state = self.proxy.get_latest_state() state = self.proxy.get_latest_state()
if state and state["drone"]["alt_rel"] >= takeoff_alt * 0.9: if state and state["drone"]["alt_rel"] >= takeoff_alt * 0.9:
alt_rel = state["drone"]["alt_rel"] alt_rel = state["drone"]["alt_rel"]
print(f"✅ 达到目标高度: {alt_rel:.1f}m") self.logger.info("达到目标高度: %.1fm", alt_rel)
break break
time.sleep(0.1) time.sleep(0.1)
else: else:
print(f"⚠️ 爬升等待超时，当前 alt_rel={state['drone']['alt_rel']:.1f}m，继续执行") alt_now = state["drone"]["alt_rel"] if state else 0
self.logger.warning("爬升等待超时，当前 alt_rel=%.1fm，继续执行", alt_now)
# ---- ⑤ 预热 + 切换 OFFBOARD ---- # ---- ⑤ 预热 + 切换 OFFBOARD ----
if mode != "OFFBOARD": if mode != "OFFBOARD":
@@ -391,7 +423,7 @@ class UAVControlLoop:
state = self.proxy.get_latest_state() state = self.proxy.get_latest_state()
current_alt = state["drone"]["alt_rel"] if state else takeoff_alt current_alt = state["drone"]["alt_rel"] if state else takeoff_alt
print(f"🔄 发送悬停 SETPOINT 预热 (alt={current_alt:.1f}m)...") self.logger.info("发送悬停 SETPOINT 预热 (alt=%.1fm)...", current_alt)
ok, ack = self.proxy.send_setpoint( ok, ack = self.proxy.send_setpoint(
x=0, y=0, z=-current_alt, yaw=0.0, x=0, y=0, z=-current_alt, yaw=0.0,
control_mode="POSITION" control_mode="POSITION"
@@ -399,12 +431,12 @@ class UAVControlLoop:
if ok: if ok:
time.sleep(0.2) # 等流建立（协议 §3.3.1 ③→④） time.sleep(0.2) # 等流建立（协议 §3.3.1 ③→④）
else: else:
print(f"⚠️ 预热 SETPOINT ACK 异常: {ack}，尝试继续...") self.logger.warning("预热 SETPOINT ACK 异常: %s，尝试继续...", ack)
print("🔄 发送 OFFBOARD 指令...") self.logger.info("发送 OFFBOARD 指令...")
ok, ack = self.proxy.send_command("OFFBOARD") ok, ack = self.proxy.send_command("OFFBOARD")
if not ok: if not ok:
print(f"❌ OFFBOARD 指令失败: {ack}") self.logger.error("OFFBOARD 指令失败: %s", ack)
return False return False
# 等待模式切换 # 等待模式切换
@@ -412,11 +444,12 @@ class UAVControlLoop:
state = self.proxy.get_latest_state() state = self.proxy.get_latest_state()
if state and state["drone"]["mode"] == "OFFBOARD": if state and state["drone"]["mode"] == "OFFBOARD":
mode = "OFFBOARD" mode = "OFFBOARD"
print("✅ 已进入 OFFBOARD 模式") self.logger.info("已进入 OFFBOARD 模式")
break break
time.sleep(0.1) time.sleep(0.1)
else: else:
print(f"❌ OFFBOARD 模式切换超时，当前 mode={state['drone']['mode'] if state else 'N/A'}") mode_str = state["drone"]["mode"] if state else "N/A"
self.logger.error("OFFBOARD 模式切换超时，当前 mode=%s", mode_str)
return False return False
# ---- ⑥ 启动独立控制线程 ---- # ---- ⑥ 启动独立控制线程 ----
@@ -425,26 +458,26 @@ class UAVControlLoop:
target=self._control_loop, daemon=True, name="uav-control" target=self._control_loop, daemon=True, name="uav-control"
) )
self.control_thread.start() self.control_thread.start()
print("✅ 控制线程已启动 (20Hz)") self.logger.info("控制线程已启动 (%.0fHz)", self.control_hz)
return True return True
# ---- 辅助方法 ---- # ---- 辅助方法 ----
def _do_arm(self): def _do_arm(self):
"""执行解锁序列""" """执行解锁序列"""
print("🔓 发送 ARM 指令...") self.logger.info("发送 ARM 指令...")
ok, ack = self.proxy.send_command("ARM") ok, ack = self.proxy.send_command("ARM")
if not ok: if not ok:
print(f"❌ ARM 指令发送失败: {ack}") self.logger.error("ARM 指令发送失败: %s", ack)
return False return False
# 等待飞控确认解锁 # 等待飞控确认解锁
for i in range(30): for i in range(30):
state = self.proxy.get_latest_state() state = self.proxy.get_latest_state()
if state and state["drone"]["armed"]: if state and state["drone"]["armed"]:
print("✅ 解锁成功") self.logger.info("解锁成功")
return True return True
time.sleep(0.1) time.sleep(0.1)
print("❌ 解锁超时（30 次轮询未确认 armed=true）") self.logger.error("解锁超时（30 次轮询未确认 armed=true）")
return False return False
# ---- 主循环调用的接口 ---- # ---- 主循环调用的接口 ----
@@ -473,12 +506,22 @@ class UAVControlLoop:
tracked = result.get("tracked", False) tracked = result.get("tracked", False)
# ------------------------------------------------ # ------------------------------------------------
# ---- 检测跟踪状态变化 ----
prev_tracked = self._last_tracked
if tracked != prev_tracked:
if tracked:
self.logger.info("目标已锁定（丢失→锁定）")
else:
self.logger.info("目标已丢失（锁定→丢失）")
self._last_tracked = tracked
# ---- ① Coast 滑行期间：PID 积分清空 ---- # ---- ① Coast 滑行期间：PID 积分清空 ----
# 卡尔曼预测阶段目标可能漂移，积分项在此累积会导致 # 卡尔曼预测阶段目标可能漂移，积分项在此累积会导致
# 目标重获瞬间积分暴冲（I-term windup） # 目标重获瞬间积分暴冲（I-term windup）
if tracked and result.get("is_predicted"): if tracked and result.get("is_predicted"):
self.pid_y.reset_integral() self.pid_y.reset_integral()
self.pid_z.reset_integral() self.pid_z.reset_integral()
self.logger.debug("Coast 预测: 清空 PID 积分")
# ---- ② 目标彻底丢失：请求控制线程执行刹车 ---- # ---- ② 目标彻底丢失：请求控制线程执行刹车 ----
# 注意：不在此处直接调 proxy.send_setpoint 以避免 REQ-REP # 注意：不在此处直接调 proxy.send_setpoint 以避免 REQ-REP
@@ -489,6 +532,7 @@ class UAVControlLoop:
self.pid_z.reset_integral() self.pid_z.reset_integral()
with self.data_lock: with self.data_lock:
self._brake_requested = True self._brake_requested = True
self.logger.info("目标丢失，请求刹车")
with self.data_lock: with self.data_lock:
self.detections = detections self.detections = detections
@@ -512,6 +556,7 @@ class UAVControlLoop:
无论是否有目标，每次迭代都发送 VELOCITY SETPOINT。 无论是否有目标，每次迭代都发送 VELOCITY SETPOINT。
无目标时速度全零（悬停刹车），满足协议 ≥20Hz 要求。 无目标时速度全零（悬停刹车），满足协议 ≥20Hz 要求。
""" """
self.logger.info("控制循环线程已启动")
while self.running: while self.running:
start = time.time() start = time.time()
@@ -525,6 +570,7 @@ class UAVControlLoop:
# 主线程超过 500ms 未更新 → 强制判丢（覆盖摄像头断流/主线程卡死） # 主线程超过 500ms 未更新 → 强制判丢（覆盖摄像头断流/主线程卡死）
if time.time() - last_update > 0.5: if time.time() - last_update > 0.5:
tracked = None tracked = None
self.logger.warning("主线程超过 500ms 未更新检测结果，强制判丢")
# ---- ③ 解析跟踪结果 → 速度指令（使用跟踪器输出的平滑中心点） ---- # ---- ③ 解析跟踪结果 → 速度指令（使用跟踪器输出的平滑中心点） ----
vx, vy, vz = self._velocity_from_tracker(tracked, w, h) vx, vy, vz = self._velocity_from_tracker(tracked, w, h)
@@ -539,8 +585,14 @@ class UAVControlLoop:
# 同时清空 PID（确保 PD 也归零） # 同时清空 PID（确保 PD 也归零）
self.pid_y.reset() self.pid_y.reset()
self.pid_z.reset() self.pid_z.reset()
self.logger.info("刹车指令已发送（控制线程执行）")
# ---- 记录当前速度指令（供外部日志读取） ----
self._last_vx, self._last_vy, self._last_vz = vx, vy, vz
# ---- ④ 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制） ---- # ---- ④ 通过 ZMQ 发送 VELOCITY SETPOINT（协议 §3.1 速度控制） ----
self.logger.debug("SETPOINT: control_mode=VELOCITY "
"vx=%.2f vy=%.2f vz=%.2f", vx, vy, vz)
ok, ack = self.proxy.send_setpoint( ok, ack = self.proxy.send_setpoint(
vx=vx, vy=vy, vz=vz, vx=vx, vy=vy, vz=vz,
yaw_rate=0.0, yaw_rate=0.0,
@@ -549,7 +601,7 @@ class UAVControlLoop:
if not ok and self.running: if not ok and self.running:
# ACK 失败记录日志，但不中断循环 # ACK 失败记录日志，但不中断循环
# recv 超时或格式错误由 _send_req 内部恢复 REQ socket # recv 超时或格式错误由 _send_req 内部恢复 REQ socket
pass self.logger.warning("SETPOINT ACK 失败: %s", ack)
# ---- ⑤ 精确控制循环频率（20Hz → 每 50ms） ---- # ---- ⑤ 精确控制循环频率（20Hz → 每 50ms） ----
elapsed = time.time() - start elapsed = time.time() - start
@@ -588,6 +640,9 @@ class UAVControlLoop:
vz = self.pid_z.update(err_y) # 上下移动（NED 中正为下） vz = self.pid_z.update(err_y) # 上下移动（NED 中正为下）
vx = 0.0 # 前后维持不变 vx = 0.0 # 前后维持不变
self.logger.debug("PID 计算: err_x=%.3f err_y=%.3f vy=%.3f vz=%.3f",
err_x, err_y, vy, vz)
# ---- 滑行预测期间：强制清空积分项 ---- # ---- 滑行预测期间：强制清空积分项 ----
# 双重保险（update_detections 中也有一道）： # 双重保险（update_detections 中也有一道）：
# 确保 coast 帧只靠 PD 出力，积分项绝不充能 # 确保 coast 帧只靠 PD 出力，积分项绝不充能
@@ -599,12 +654,13 @@ class UAVControlLoop:
def stop(self): def stop(self):
"""停止控制线程，发送刹车指令，关闭 ZMQ 代理""" """停止控制线程，发送刹车指令，关闭 ZMQ 代理"""
self.logger.info("正在停止控制线程...")
self.running = False self.running = False
if self.control_thread: if self.control_thread:
self.control_thread.join(timeout=2.0) self.control_thread.join(timeout=2.0)
# 发送零速刹车指令 # 发送零速刹车指令
print("🛑 发送刹车指令...") self.logger.info("发送刹车指令...")
self.proxy.send_setpoint( self.proxy.send_setpoint(
vx=0.0, vy=0.0, vz=0.0, vx=0.0, vy=0.0, vz=0.0,
yaw_rate=0.0, yaw_rate=0.0,
@@ -616,11 +672,21 @@ class UAVControlLoop:
self._tracked_result = None self._tracked_result = None
self.proxy.close() self.proxy.close()
print("控制已停止，ZMQ 代理已关闭，跟踪器已重置") self.logger.info("控制已停止，ZMQ 代理已关闭，跟踪器已重置")
# ==================== 主程序入口 ====================== # ==================== 主程序入口 ======================
def main(): def main():
logging.basicConfig(
level=logging.INFO,
format='%(asctime)s - %(name)s - %(levelname)s - %(message)s',
handlers=[
logging.FileHandler('./log/npu_infer.log'), # 写入文件
logging.StreamHandler()
]
)
main_logger = logging.getLogger("Main")
# 默认加载同目录下的 config.json # 默认加载同目录下的 config.json
config_file = "config.json" config_file = "config.json"
@@ -628,12 +694,12 @@ def main():
if len(sys.argv) > 1: if len(sys.argv) > 1:
config_file = sys.argv[1] config_file = sys.argv[1]
print(f"📖 正在从 {config_file} 加载系统配置...") main_logger.info("正在从 %s 加载系统配置...", config_file)
try: try:
with open(config_file, "r", encoding="utf-8") as f: with open(config_file, "r", encoding="utf-8") as f:
cfg = json.load(f) cfg = json.load(f)
except Exception as e: except Exception as e:
print(f"❌ 读取配置文件失败: {e}") main_logger.error("读取配置文件失败: %s", e)
sys.exit(1) sys.exit(1)
# 实例化各子系统并注入配置 # 实例化各子系统并注入配置
@@ -643,10 +709,10 @@ def main():
# 启动无人机连接（RouterProxy 启动 + 起飞序列） # 启动无人机连接（RouterProxy 启动 + 起飞序列）
if not controller.start_uav(): if not controller.start_uav():
print("❌ 飞控启动失败，退出") main_logger.error("飞控启动失败，退出")
sys.exit(1) sys.exit(1)
print("🚀 NPU 精准推理 [ZMQ Router 协议版] 主循环启动...") main_logger.info("NPU 精准推理 [ZMQ Router 协议版] 主循环启动...")
loop_count = 0 loop_count = 0
try: try:
@@ -721,27 +787,35 @@ def main():
# 6. 周期性打印 Router + Tracker 状态（每 100 帧） # 6. 周期性打印 Router + Tracker 状态（每 100 帧）
if loop_count % 100 == 0: if loop_count % 100 == 0:
state = controller.proxy.get_latest_state() state = controller.proxy.get_latest_state()
tracker_info = (f" TK:{'✅' if tracked else '❌'}" is_coast = track_res.get("is_predicted", False) if track_res else False
f" ID:{track_res.get('primary_id','?')}" lost_frames = track_res.get("lost_frames", 0) if track_res else 0
f" Lost:{track_res.get('lost_frames',0)}f" n_active = track_res.get("n_active", 0) if track_res else 0
f" {'PRED' if track_res.get('is_predicted') else 'MEAS'}" primary_id = track_res.get("primary_id", "?") if track_res else "?"
f" Act:{track_res.get('n_active',0)}") tracker_info = (f"TK:{'✅' if tracked else '❌'}"
f" ID:{primary_id}"
f" Lost:{lost_frames}f"
f" {'PRED' if is_coast else 'MEAS'}"
f" Act:{n_active}")
if state: if state:
d = state["drone"] d = state["drone"]
age_us, _ = controller.proxy.get_state_freshness(state) age_us, _ = controller.proxy.get_state_freshness(state)
age_ms = (age_us or 0) / 1000 age_ms = (age_us or 0) / 1000
print(f"📡 Router: mode={d['mode']} armed={d['armed']} " main_logger.info(
f"alt={d['alt_rel']:.1f}m spd={d['ground_speed']:.1f}m/s " "📡 Router: mode=%s armed=%s alt=%.1fm spd=%.1fm/s "
f"batt={d['battery']:.0f}% age={age_ms:.0f}ms" "batt=%.0f%% age=%.0fms v=(%.1f,%.1f,%.1f) %s",
f"{tracker_info}", d["mode"], d["armed"], d["alt_rel"],
end="" if loop_count % 500 != 0 else "\n") d["ground_speed"], d["battery"], age_ms,
controller._last_vx, controller._last_vy,
controller._last_vz, tracker_info
)
except KeyboardInterrupt: except KeyboardInterrupt:
print("\n👋 接收到终止信号，安全退出中...") main_logger.info("接收到终止信号，安全退出中...")
print() # 换行，使输出整洁
finally: finally:
controller.stop() controller.stop()
streamer.release() streamer.release()
print("程序运行结束。") main_logger.info("程序运行结束。")
if __name__ == "__main__": if __name__ == "__main__":