增加了打印的日志

drone_controller/router_proxy.py

@@ -89,6 +89,9 @@ class RouterProxy:
         self._latest_state = None
         self._state_lock = threading.Lock()
 
+        # ---- 首次 STATE 标记 ----
+        self._first_state_received = False
+
         # ---- SUB 监听线程 ----
         self._running = False
         self._sub_thread = None
@@ -107,6 +110,21 @@ class RouterProxy:
             self._seq += 1
             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):
         """
         REQ 超时/异常后重建 socket
@@ -224,21 +242,27 @@ class RouterProxy:
             (ok: bool, ack_dict: dict)
             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:
             self.req.send_string(json.dumps(msg))
             ack_str = self.req.recv_string()
             ack = json.loads(ack_str)
             ok = ack.get("status") == "OK"
+            logger.info("ACK received for id=%s, status=%s",
+                        ack.get("ref_id"), ack.get("status"))
             if not ok:
                 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
         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()
             return False, {"status": "TIMEOUT", "message": "recv timeout"}
         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()
             return False, {"status": "FAIL", "message": str(e)}
 
@@ -278,9 +302,13 @@ class RouterProxy:
             age_us = now_us - state["drone"]["last_update_us"]
         except (KeyError, TypeError):
             return False
+        if age_us >= stale_threshold_us:
             if age_us >= self.STALE_THRESHOLD_WARN_US:
-            logger.warning("STATE 严重过期: age=%.2fs（阈值=%.1fs）",
-                           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
 
     def get_state_freshness(self, state):
@@ -351,6 +379,25 @@ class RouterProxy:
                 if msg_type == "STATE":
                     with self._state_lock:
                         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:
                         try:
                             self._state_callback(data)
@@ -375,7 +422,8 @@ class RouterProxy:
                     # SUB 通道上的 QUERY_REPLY 暂不处理
                     pass
 
-                # 其它消息类型可扩展
+                else:
+                    logger.warning("收到未知消息类型: %s", msg_type)
 
             except zmq.Again:
                 continue

infer_camera_modular.py

@@ -7,6 +7,7 @@ import queue
 import socket
 import json
 import sys
+import logging
 
 # ================== 昇腾 NPU SDK（保护性导入） =====================
 # 开发机可能无昇腾环境，try-except 确保 import 错误仅告警不崩溃。
@@ -31,6 +32,7 @@ class VideoStreaming:
     """ 📡 视频流采集与后台图传模块（支持 RTSP 自动重连） """
 
     def __init__(self, video_cfg, net_cfg):
+        self.logger = logging.getLogger("VideoStream")
         self.video_source = video_cfg.get("video_source", 0)
         self.jpeg_quality = video_cfg.get("jpeg_quality", 75)
         self.gs_ip = net_cfg.get("ground_station_ip", "127.0.0.1")
@@ -69,17 +71,21 @@ class VideoStreaming:
             ret, frame = self.cap.read()
             if not ret:
                 # 读取失败，关闭并标记需重连
-                print("⚠️ RTSP 读取失败，准备重连...")
+                self.logger.warning("RTSP 读取失败，准备重连...")
                 self.cap.release()
                 self.cap = None
                 continue
 
             # 清空旧帧，只保留最新一帧
+            dropped = 0
             while not self.frame_buffer.empty():
                 try:
                     self.frame_buffer.get_nowait()
+                    dropped += 1
                 except queue.Empty:
                     break
+            if dropped > 0:
+                self.logger.debug("丢弃旧帧 %d 帧（缓冲区刷新）", dropped)
             self.frame_buffer.put(frame)
 
         if self.cap is not None:
@@ -88,7 +94,7 @@ class VideoStreaming:
     def _open_camera(self):
         """ 打开摄像头或 RTSP 流，并设置低延迟参数 """
         try:
-            print(f"📷 正在连接视频源: {self.video_source}")
+            self.logger.info("正在连接视频源: %s", self.video_source)
             self.cap = cv2.VideoCapture(self.video_source)
 
             if self.is_rtsp:
@@ -102,7 +108,7 @@ class VideoStreaming:
                 if "?" not in self.video_source:
                     self.video_source += "?tcp"
                     # 注意：有些相机需要重新初始化，这里简单打印提示
-                    print("💡 建议使用 TCP 传输: 在 RTSP URL 后添加 ?tcp")
+                    self.logger.info("建议使用 TCP 传输: 在 RTSP URL 后添加 ?tcp")
             else:
                 # USB 摄像头也可设置缓冲区
                 self.cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)
@@ -112,11 +118,11 @@ class VideoStreaming:
             # self.cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 480)
 
             if self.cap.isOpened():
-                print("✅ 视频源连接成功")
+                self.logger.info("视频源连接成功: %s", self.video_source)
             else:
-                print("❌ 视频源打开失败")
+                self.logger.error("视频源打开失败: %s", self.video_source)
         except Exception as e:
-            print(f"❌ 打开视频源异常: {e}")
+            self.logger.error("打开视频源异常: %s", e)
             self.cap = None
 
     def read_frame(self):
@@ -137,10 +143,12 @@ class VideoStreaming:
             pass
 
     def _udp_stream_sender_worker(self):
-        """ 原图传发送线程（未改动） """
+        """ 原图传发送线程（增加帧统计日志） """
         sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
         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:
             try:
@@ -153,12 +161,16 @@ class VideoStreaming:
                     continue
                 data = img_encode.tobytes()
                 if len(data) > 65000:
+                    self.logger.debug("图像数据过大 (%d bytes), 跳过发送", len(data))
                     continue
                 sock.sendto(data, server_address)
+                send_count += 1
+                if send_count % 100 == 0:
+                    self.logger.info("图传统计: 已发送 %d 帧 UDP 数据包", send_count)
             except queue.Empty:
                 continue
             except Exception as e:
-                print(f"❌ 图传线程异常: {e}")
+                self.logger.exception("图传线程异常: %s", e)
                 time.sleep(0.1)
         sock.close()
 
@@ -176,6 +188,7 @@ class YOLO26UAVInfer:
     """ 🚀 昇腾 NPU 推理与图像后处理模块 """
 
     def __init__(self, model_cfg):
+        self.logger = logging.getLogger("YOLO")
         model_path = model_cfg.get("model_path", "./om/yolo26n-balloon.om")
         device_id = model_cfg.get("device_id", 0)
         self.conf_threshold = model_cfg.get("conf_threshold", 0.25)
@@ -186,6 +199,8 @@ class YOLO26UAVInfer:
         self.session = InferSession(device_id, model_path)
         self.input_width = 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)):
         shape = img.shape[:2]  # [height, width]
@@ -208,6 +223,8 @@ class YOLO26UAVInfer:
         input_tensor = np.transpose(input_tensor, (2, 0, 1))
         input_tensor = np.expand_dims(input_tensor, axis=0)
         input_tensor = np.ascontiguousarray(input_tensor)
+        self.logger.debug("预处理: 输入形状 %s -> %s",
+                          str(frame.shape), str(input_tensor.shape))
         return input_tensor, ratio, dwdh
 
     def postprocess(self, outputs, ratio, dwdh, orig_shape):
@@ -229,6 +246,11 @@ class YOLO26UAVInfer:
                 x2 = max(0, min(x2, orig_shape[1]))
                 y2 = max(0, min(y2, orig_shape[0]))
                 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
 
     def draw_boxes(self, img, detections):
@@ -264,6 +286,8 @@ class UAVControlLoop:
     """
 
     def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg):
+        self.logger = logging.getLogger("UAVControlLoop")
+
         # ---- 保存 config（供 start_uav 使用如 takeoff_alt 等） ----
         self.fc_cfg = fc_cfg
         # -----------------------------------------------------------
@@ -287,6 +311,8 @@ class UAVControlLoop:
             min_hits=tracker_cfg.get("min_hits", 3),
         )
         self._tracked_result = None   # 跟踪结果（锁保护）
+        # 记录上一次跟踪状态，用于检测状态变化
+        self._last_tracked = False
         # ------------------------------------------------
 
         # 控制参数（按协议改为 20Hz，§4.1 要求 ≥20Hz）
@@ -309,6 +335,11 @@ class UAVControlLoop:
         # ---- 安全刹车请求标志（主线程判丢 → 控制线程执行刹车） ----
         self._brake_requested = False
 
+        # ---- 最近一次发送的速度指令（用于周期性日志打印） ----
+        self._last_vx = 0.0
+        self._last_vy = 0.0
+        self._last_vz = 0.0
+
     def start_uav(self):
         """连接 Router，执行标准起飞序列（协议 §3.3.1）
 
@@ -322,19 +353,19 @@ class UAVControlLoop:
         """
         # ---- ① 启动 ZMQ 代理 ----
         self.proxy.start()
-        print("🔄 RouterProxy 已启动，等待 Router 状态推送...")
+        self.logger.info("RouterProxy 已启动，等待 Router 状态推送...")
 
         # ---- ② 等待收到第一条 STATE ----
         state = None
         for i in range(50):  # 最多等 5 秒
             state = self.proxy.get_latest_state()
             if state is not None:
-                print("✅ 收到 Router 状态推送")
+                self.logger.info("收到 Router 状态推送")
                 break
             time.sleep(0.1)
 
         if state is None:
-            print("❌ 无法获取飞机状态，请检查 Router 是否运行，"
+            self.logger.error("无法获取飞机状态，请检查 Router 是否运行，"
                               "且 SUB 端口 (tcp://127.0.0.1:5556) 可连通")
             return False
 
@@ -344,8 +375,8 @@ class UAVControlLoop:
         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")
+        self.logger.info("初始状态: mode=%s armed=%s alt_rel=%.1fm state_age=%.2fs",
+                         mode, armed, alt_rel, age_s)
 
         # ---- ③ 解锁（如未解锁） ----
         if not armed:
@@ -358,20 +389,20 @@ class UAVControlLoop:
         takeoff_alt = self.fc_cfg.get("takeoff_alt", 5.0)
 
         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(
                 action="TAKEOFF", alt=takeoff_alt,
                 alt_frame="RELATIVE", speed=3.0
             )
             if not ok:
-                print(f"⚠️ TAKEOFF 指令 ACK 失败: {ack}，尝试通过 SETPOINT 爬升...")
+                self.logger.warning("TAKEOFF 指令 ACK 失败: %s，尝试通过 SETPOINT 爬升...", ack)
                 # 降级方案：用 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}")
+                    self.logger.error("爬升 SETPOINT 也失败: %s", ack)
                     return False
 
             # 等待爬升
@@ -379,11 +410,12 @@ class UAVControlLoop:
                 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")
+                    self.logger.info("达到目标高度: %.1fm", alt_rel)
                     break
                 time.sleep(0.1)
             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 ----
         if mode != "OFFBOARD":
@@ -391,7 +423,7 @@ class UAVControlLoop:
             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)...")
+            self.logger.info("发送悬停 SETPOINT 预热 (alt=%.1fm)...", current_alt)
             ok, ack = self.proxy.send_setpoint(
                 x=0, y=0, z=-current_alt, yaw=0.0,
                 control_mode="POSITION"
@@ -399,12 +431,12 @@ class UAVControlLoop:
             if ok:
                 time.sleep(0.2)  # 等流建立（协议 §3.3.1 ③→④）
             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")
             if not ok:
-                print(f"❌ OFFBOARD 指令失败: {ack}")
+                self.logger.error("OFFBOARD 指令失败: %s", ack)
                 return False
 
             # 等待模式切换
@@ -412,11 +444,12 @@ class UAVControlLoop:
                 state = self.proxy.get_latest_state()
                 if state and state["drone"]["mode"] == "OFFBOARD":
                     mode = "OFFBOARD"
-                    print("✅ 已进入 OFFBOARD 模式")
+                    self.logger.info("已进入 OFFBOARD 模式")
                     break
                 time.sleep(0.1)
             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
 
         # ---- ⑥ 启动独立控制线程 ----
@@ -425,26 +458,26 @@ class UAVControlLoop:
             target=self._control_loop, daemon=True, name="uav-control"
         )
         self.control_thread.start()
-        print("✅ 控制线程已启动 (20Hz)")
+        self.logger.info("控制线程已启动 (%.0fHz)", self.control_hz)
         return True
 
     # ---- 辅助方法 ----
 
     def _do_arm(self):
         """执行解锁序列"""
-        print("🔓 发送 ARM 指令...")
+        self.logger.info("发送 ARM 指令...")
         ok, ack = self.proxy.send_command("ARM")
         if not ok:
-            print(f"❌ ARM 指令发送失败: {ack}")
+            self.logger.error("ARM 指令发送失败: %s", ack)
             return False
         # 等待飞控确认解锁
         for i in range(30):
             state = self.proxy.get_latest_state()
             if state and state["drone"]["armed"]:
-                print("✅ 解锁成功")
+                self.logger.info("解锁成功")
                 return True
             time.sleep(0.1)
-        print("❌ 解锁超时（30 次轮询未确认 armed=true）")
+        self.logger.error("解锁超时（30 次轮询未确认 armed=true）")
         return False
 
     # ---- 主循环调用的接口 ----
@@ -473,12 +506,22 @@ class UAVControlLoop:
         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 积分清空 ----
         # 卡尔曼预测阶段目标可能漂移，积分项在此累积会导致
         # 目标重获瞬间积分暴冲（I-term windup）
         if tracked and result.get("is_predicted"):
             self.pid_y.reset_integral()
             self.pid_z.reset_integral()
+            self.logger.debug("Coast 预测: 清空 PID 积分")
 
         # ---- ② 目标彻底丢失：请求控制线程执行刹车 ----
         # 注意：不在此处直接调 proxy.send_setpoint 以避免 REQ-REP
@@ -489,6 +532,7 @@ class UAVControlLoop:
             self.pid_z.reset_integral()
             with self.data_lock:
                 self._brake_requested = True
+                self.logger.info("目标丢失，请求刹车")
 
         with self.data_lock:
             self.detections = detections
@@ -512,6 +556,7 @@ class UAVControlLoop:
         无论是否有目标，每次迭代都发送 VELOCITY SETPOINT。
         无目标时速度全零（悬停刹车），满足协议 ≥20Hz 要求。
         """
+        self.logger.info("控制循环线程已启动")
         while self.running:
             start = time.time()
 
@@ -525,6 +570,7 @@ class UAVControlLoop:
             # 主线程超过 500ms 未更新 → 强制判丢（覆盖摄像头断流/主线程卡死）
             if time.time() - last_update > 0.5:
                 tracked = None
+                self.logger.warning("主线程超过 500ms 未更新检测结果，强制判丢")
 
             # ---- ③ 解析跟踪结果 → 速度指令（使用跟踪器输出的平滑中心点） ----
             vx, vy, vz = self._velocity_from_tracker(tracked, w, h)
@@ -539,8 +585,14 @@ class UAVControlLoop:
                     # 同时清空 PID（确保 PD 也归零）
                     self.pid_y.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 速度控制） ----
+            self.logger.debug("SETPOINT: control_mode=VELOCITY "
+                              "vx=%.2f vy=%.2f vz=%.2f", vx, vy, vz)
             ok, ack = self.proxy.send_setpoint(
                 vx=vx, vy=vy, vz=vz,
                 yaw_rate=0.0,
@@ -549,7 +601,7 @@ class UAVControlLoop:
             if not ok and self.running:
                 # ACK 失败记录日志，但不中断循环
                 # recv 超时或格式错误由 _send_req 内部恢复 REQ socket
-                pass
+                self.logger.warning("SETPOINT ACK 失败: %s", ack)
 
             # ---- ⑤ 精确控制循环频率（20Hz → 每 50ms） ----
             elapsed = time.time() - start
@@ -588,6 +640,9 @@ class UAVControlLoop:
         vz = self.pid_z.update(err_y)   # 上下移动（NED 中正为下）
         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 中也有一道）：
         # 确保 coast 帧只靠 PD 出力，积分项绝不充能
@@ -599,12 +654,13 @@ class UAVControlLoop:
 
     def stop(self):
         """停止控制线程，发送刹车指令，关闭 ZMQ 代理"""
+        self.logger.info("正在停止控制线程...")
         self.running = False
         if self.control_thread:
             self.control_thread.join(timeout=2.0)
 
         # 发送零速刹车指令
-        print("🛑 发送刹车指令...")
+        self.logger.info("发送刹车指令...")
         self.proxy.send_setpoint(
             vx=0.0, vy=0.0, vz=0.0,
             yaw_rate=0.0,
@@ -616,11 +672,21 @@ class UAVControlLoop:
         self._tracked_result = None
 
         self.proxy.close()
-        print("控制已停止，ZMQ 代理已关闭，跟踪器已重置")
+        self.logger.info("控制已停止，ZMQ 代理已关闭，跟踪器已重置")
 
 
 # ==================== 主程序入口 ======================
 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_file = "config.json"
 
@@ -628,12 +694,12 @@ def main():
     if len(sys.argv) > 1:
         config_file = sys.argv[1]
 
-    print(f"📖 正在从 {config_file} 加载系统配置...")
+    main_logger.info("正在从 %s 加载系统配置...", config_file)
     try:
         with open(config_file, "r", encoding="utf-8") as f:
             cfg = json.load(f)
     except Exception as e:
-        print(f"❌ 读取配置文件失败: {e}")
+        main_logger.error("读取配置文件失败: %s", e)
         sys.exit(1)
 
     # 实例化各子系统并注入配置
@@ -643,10 +709,10 @@ def main():
 
     # 启动无人机连接（RouterProxy 启动 + 起飞序列）
     if not controller.start_uav():
-        print("❌ 飞控启动失败，退出")
+        main_logger.error("飞控启动失败，退出")
         sys.exit(1)
 
-    print("🚀 NPU 精准推理 [ZMQ Router 协议版] 主循环启动...")
+    main_logger.info("NPU 精准推理 [ZMQ Router 协议版] 主循环启动...")
     loop_count = 0
 
     try:
@@ -721,27 +787,35 @@ def main():
             # 6. 周期性打印 Router + Tracker 状态（每 100 帧）
             if loop_count % 100 == 0:
                 state = controller.proxy.get_latest_state()
+                is_coast = track_res.get("is_predicted", False) if track_res else False
+                lost_frames = track_res.get("lost_frames", 0) if track_res else 0
+                n_active = track_res.get("n_active", 0) if track_res else 0
+                primary_id = track_res.get("primary_id", "?") if track_res else "?"
                 tracker_info = (f"TK:{'✅' if tracked else '❌'}"
-                                f" ID:{track_res.get('primary_id','?')}"
-                                f" Lost:{track_res.get('lost_frames',0)}f"
-                                f" {'PRED' if track_res.get('is_predicted') else 'MEAS'}"
-                                f" Act:{track_res.get('n_active',0)}")
+                                f" ID:{primary_id}"
+                                f" Lost:{lost_frames}f"
+                                f" {'PRED' if is_coast else 'MEAS'}"
+                                f" Act:{n_active}")
                 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}% age={age_ms:.0f}ms"
-                          f"{tracker_info}",
-                          end="" if loop_count % 500 != 0 else "\n")
+                    main_logger.info(
+                        "📡 Router: mode=%s armed=%s alt=%.1fm spd=%.1fm/s "
+                        "batt=%.0f%% age=%.0fms v=(%.1f,%.1f,%.1f) %s",
+                        d["mode"], d["armed"], d["alt_rel"],
+                        d["ground_speed"], d["battery"], age_ms,
+                        controller._last_vx, controller._last_vy,
+                        controller._last_vz, tracker_info
+                    )
 
     except KeyboardInterrupt:
-        print("\n👋 接收到终止信号，安全退出中...")
+        main_logger.info("接收到终止信号，安全退出中...")
+        print()  # 换行，使输出整洁
     finally:
         controller.stop()
         streamer.release()
-        print("程序运行结束。")
+        main_logger.info("程序运行结束。")
 
 
 if __name__ == "__main__":