2026.6.3小更新

README.md

@@ -1,22 +1,25 @@
-# 这是一个仅供***个人瞎搞***的东西
+# 这是一个***小白瞎搞***的东西,仅供参考(我甚至觉得不能作为参考)
 
 这些会部署在香橙派AI Pro上，模型为*YOLOv26n*
 
-### 说明下各文件的作用
+## 说明下各文件的作用
+#### *python部分*
+#### **零散脚本**
 - check.py:确认输入视频帧率，用于性能测试的;
 - control.py:PID计算和MAVLink指令发送方法的定义(已废弃);
 - fc_test:获取飞控信息的测试脚本;
 - infer_camera_direct.py:通过摄像头检测目标并调用方法控制无人机;
-- infer_camera_modular.py:模块化处理过的摄像头检测与控制(未测试)
-- infer_camera_video.py:使用视频检测目标并调用方法控制无人机(用于测试);
 - infer.py:使用图片检测目标，仅用于测试环境;
-#### 无人机控制部分:
+- infer_camera_modular.py:模块化处理过的摄像头检测与控制(未测试)
-- base_control.py:定义了部分基本动作的方法
+- infer_camera_video.py:使用视频检测目标并调用方法控制无人机(用于测试,未调参)
-#### 视觉跟踪业务:
+- gent_mission.py:尝试将视觉追踪与自动巡点飞行相结合(未测试，这应该是本项目一阶段的最终预期)
-- pid_counter.py:抽离出来的PID计算器
-#### 自动巡点飞行业务:
-- mission_manager.py:自动巡点飞行的控制模块
 - run_mission.py:自动巡点飞行的业务测试(未测试)
-#### 目录：
+#### drone_controller:控制模块
+- base_control.py:定义了部分基本动作的方法
+- pid_counter.py:抽离出来的PID计算器
+- mission_manager.py:自动巡点飞行的控制模块
+#### 其他目录以及文件：
 - om:推理使用的.om文件
-- drone_controller:控制模块
+- asset:用于基础测试的文件
+- config.json:配置文件
+- start_npu_infer:一个.sh文件,计划后续将整个脚本作为一个服务

drone_controller/mission_manager.py

@@ -8,149 +8,176 @@ class MissionState(Enum):
     ARMING = 1  # 解锁中
     TAKEOFF = 2  # 自动起飞中
     NAVIGATING = 3  # 航点巡航导航
-    HOLD_TASK = 4  # 到达航点后的原地任务悬停
+    HOLD_TASK = 4  # 到达航点后的原地搜索/悬停
-    RETURNING = 5  # 返航或直接准备降落
+    VISUAL_TRACKING = 5  # 🎯 视觉追踪中（由视觉主循环接管控制权）
-    LANDING = 6  # 自动降落中
+    RETURNING = 6  # 返航中
-    FINISHED = 7  # 任务结束/已上锁
+    LANDING = 7  # 自动降落中
-    HOLD_FINAL = 8  # 结束巡点后原地悬停（当 return_to_home 为 False 时使用）
+    FINISHED = 8  # 任务结束/已上锁
+    HOLD_FINAL = 9  # 结束巡点后原地悬停（当 return_to_home 为 False 时使用）
+
 
 class FailsafeTriggered(Exception):
     """自定义安全异常：当安全员人工介入切出自控模式时抛出，用于暴力熔断主循环"""
     pass
 
+
 class MissionManager:
     def __init__(self, drone, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0,
                  return_to_home=True):
         """
         :param drone: 实例化的 DroneController 底层驱动对象
-        :param waypoints: 航点列表，格式为 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ... ]
+        :param waypoints: 航点列表 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ... ]
-                          注意：NED 坐标系中，Z轴向上为负，所以起飞或目标高度应为负值
+        :param target_altitude: 默认起飞高度（米，NED中向上为负值）
-        :param target_altitude: 初始起飞高度（米，正数，内部会自动转为NED负值）
+        :param arrival_radius: 航点到达判定半径（米）
-        :param arrival_radius: 判定航点到达的三维欧氏距离阈值（米）
+        :param hold_duration: 抵达巡点后，没有发现目标时的最大搜寻/悬停时间（秒）
-        :param hold_duration: 在每个航点到达后，执行任务（或悬停）的停留时间（秒）
+        :param return_to_home: 航点巡完后是否返航，False则在最后一个点原地悬停
-        :param return_to_home: 巡点结束后是否返回起始点降落。如果为 False，则在最后一个航点原地悬停。
         """
         self.drone = drone
         self.waypoints = waypoints
-        self.target_altitude = -abs(target_altitude)  # 强制转为 NED 负值
+        self.target_altitude = target_altitude if target_altitude < 0 else -target_altitude
         self.arrival_radius = arrival_radius
         self.hold_duration = hold_duration
         self.return_to_home = return_to_home
 
         self.state = MissionState.INIT
         self.current_wp_index = 0
-        self.hold_start_time = 0.0
 
-        # 记录起始点坐标，用于返航
+        # 计时器定义
+        self.hold_start_time = 0.0
+        self.tracking_start_time = 0.0
+
+        # 起始点（Home）坐标缓存
         self.home_x = 0.0
         self.home_y = 0.0
-        self.home_z = self.target_altitude
+        self.home_z = 0.0
 
-    def get_distance(self, target_wp):
+    def _is_tracking_task_complete(self):
-        """计算当前无人机局部 NED 坐标到目标航点的三维欧氏距离"""
-        curr_x, curr_y, curr_z = self.drone.local_position
-        dx = target_wp['x'] - curr_x
-        dy = target_wp['y'] - curr_y
-        dz = target_wp['z'] - curr_z
-        return math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
-
-    def update(self):
         """
-        状态机核心主循环，需由上位机主程序以 5Hz~10Hz 的高频流式调用。
+        🛠️ 预留接口：判断视觉追踪任务是否完成
-        包含链路维持、安全熔断以及新增的原地悬停/返航分支。
+        你可以根据需求修改内部逻辑。例如：
+        - 当前：假设持续追踪 5 秒视为完成任务。
+        - 后续扩展：可以引入与目标的物理距离、传感器状态等。
         """
-        # 1. 专属安全熔断机制：一旦检测到场外安全员切回手动模式，立即熔断
+        tracking_duration = time.time() - self.tracking_start_time
-        if self.drone.current_mode not in ['GUIDED', 'OFFBOARD'] and self.state != MissionState.INIT:
+        target_hold_time = 5.0  # 默认设定5秒
-            print(f"\n⚠️ [安全熔断] 检测到飞控退出自动控制模式(当前模式: {self.drone.current_mode})！上位机状态机退出。")
-            self.state = MissionState.FINISHED
-            return False
 
-        # 2. 状态机逻辑分支
+        if tracking_duration >= target_hold_time:
+            return True
+        return False
+
+    def update(self, target_detected=False):
+        """
+        状态机高频核心业务逻辑，由外部主循环驱动
+        :param target_detected: 当前帧是否发现了视觉追踪目标
+        """
+        # 0. 越权检查（最强人肉兜底）：只要切出自主控制模式，立刻暴力熔断
+        if self.state not in [MissionState.INIT, MissionState.FINISHED]:
+            if self.drone.current_mode not in ['GUIDED', 'OFFBOARD']:
+                print(f"\n⚠️ [Failsafe] 检测到飞控模式被切为: {self.drone.current_mode}! 外部人工强行接管，熔断任务！")
+                self.state = MissionState.FINISHED
+                raise FailsafeTriggered("安全员强行接管")
+
+        # 1. INIT: 等待飞控连接并获取当前位置作为 Home 点
         if self.state == MissionState.INIT:
             if self.drone.is_connected:
-                print("\n[通信成功] 无人机已连接，准备记录起始点并解锁...")
+                print("[INIT] 飞控已连上。获取当前坐标为起始点...")
-                # 记录解锁时的局部位置作为 Home 点
+                self.home_x, self.home_y, self.home_z = self.drone.local_position
-                hx, hy, _ = self.drone.local_position
+                print(f"[INIT] 起始点坐标: ({self.home_x:.2f}, {self.home_y:.2f}, {self.home_z:.2f})")
-                self.home_x = hx
+                print("[INIT] 正在下发 GUIDED/OFFBOARD 模式并尝试解锁...")
-                self.home_y = hy
-                self.state = MissionState.ARMING
-            else:
-                print("\r[等待中] 等待飞控心跳连接...", end="", flush=True)
 
-        elif self.state == MissionState.ARMING:
+                target_mode = 'GUIDED' if 'guided' in self.drone.current_mode.lower() or True else 'OFFBOARD'
-            print("\r[指令下发] 正在尝试解锁电机...", end="", flush=True)
-            if self.drone.is_armed:
-                print("\n[成功] 电机已解锁！准备切入控制模式并起飞。")
-                # 引导无人机切入 GUIDED 或 OFFBOARD
-                target_mode = 'OFFBOARD' if 'PX4' in getattr(self.drone, 'firmware_type', 'PX4') else 'GUIDED'
                 self.drone.set_mode(target_mode)
-                self.state = MissionState.TAKEOFF
-            else:
                 self.drone.arm()
-                time.sleep(0.5)
+                self.state = MissionState.ARMING
 
+        # 2. ARMING: 监测解锁状态
+        elif self.state == MissionState.ARMING:
+            if self.drone.is_armed and self.drone.current_mode in ['GUIDED', 'OFFBOARD']:
+                print("\n[解锁成功] 正在下发自动起飞指令...")
+                # 下发位置到期望高度
+                self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
+                self.state = MissionState.TAKEOFF
+
+        # 3. TAKEOFF: 自动起飞爬升判定
         elif self.state == MissionState.TAKEOFF:
-            # 虚拟/流式起飞：高频下发起飞期望高度，维持链路活性
+            current_z = self.drone.local_position[2]
+            # 持续发送起飞点位置，激活控制链路
             self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
 
-            # 判定是否到达起飞高度
+            # NED 坐标系下 Z 为负数，判断是否接近目标高度
-            _, _, curr_z = self.drone.local_position
+            if abs(current_z - self.target_altitude) < self.arrival_radius:
-            if abs(curr_z - self.target_altitude) < self.arrival_radius:
+                print("\n[起飞完成] 达到预定高度。开始前往第 1 个巡航航点...")
-                print(f"\n[完成] 已到达安全起飞高度: {abs(curr_z):.2f}m。开始巡航任务...")
+                self.state = MissionState.NAVIGATING
-                if len(self.waypoints) > 0:
+                self.current_wp_index = 0
-                    self.current_wp_index = 0
-                    self.state = MissionState.NAVIGATING
-                else:
-                    print("⚠️ 警告: 航点列表为空！")
-                    if self.return_to_home:
-                        self.state = MissionState.RETURNING
-                    else:
-                        self.state = MissionState.HOLD_FINAL
-            else:
-                print(f"\r[起飞中] 当前高度: {abs(curr_z):.2f}m / 目标: {abs(self.target_altitude):.2f}m", end="",
-                      flush=True)
 
+        # 4. NAVIGATING: 航点导航巡航
         elif self.state == MissionState.NAVIGATING:
+            if not self.waypoints or self.current_wp_index >= len(self.waypoints):
+                # 航点全部飞完
+                if self.return_to_home:
+                    print("\n[巡航完成] 所有航点已巡视完毕。正在返回起始点上方...")
+                    self.state = MissionState.RETURNING
+                else:
+                    print("\n[巡航完成] 所有航点已巡视完毕。根据配置切换至末端就地悬停...")
+                    self.state = MissionState.HOLD_FINAL
+                return
+
+            # 获取当前目标航点
             wp = self.waypoints[self.current_wp_index]
-            # “零期望命令”防失控刹车：流式高频持续推流当前目标点
             self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
 
-            dist = self.get_distance(wp)
+            # 计算水平及高度距离
-            print(f"\r[任务中] 正在飞往航点 [{self.current_wp_index + 1}] | 剩余距离: {dist:.2f}m", end="", flush=True)
+            dx = wp['x'] - self.drone.local_position[0]
+            dy = wp['y'] - self.drone.local_position[1]
+            dz = wp['z'] - self.drone.local_position[2]
+            dist_xyz = math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
 
-            if dist < self.arrival_radius:
-                print(f"\n[到达] 已精准触达航点 [{self.current_wp_index + 1}]，开启原地任务阶段。")
-                self.hold_start_time = time.time()
-                self.state = MissionState.HOLD_TASK
-
-        elif self.state == MissionState.HOLD_TASK:
-            wp = self.waypoints[self.current_wp_index]
-            # 悬停期间同样高频重发当前坐标，让飞控稳定在原地
-            self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
-
-            elapsed = time.time() - self.hold_start_time
             print(
-                f"\r[任务中] 航点 [{self.current_wp_index + 1}] 原地悬停检测中... 已耗时: {elapsed:.1f}s / {self.hold_duration}s",
+                f"\n\r[巡航中] 前往航点 [{self.current_wp_index + 1}/{len(self.waypoints)}] | 距离目标: {dist_xyz:.2f}m",
                 end="", flush=True)
 
-            if elapsed >= self.hold_duration:
+            if dist_xyz < self.arrival_radius:
-                print(f"\n[完成] 航点 [{self.current_wp_index + 1}] 任务执行完毕。")
+                print(f"\n[到达航点 {self.current_wp_index + 1}] 进入原地悬停与视觉检索阶段...")
-                # 切换下一个点
+                self.state = MissionState.HOLD_TASK
-                self.current_wp_index += 1
+                self.hold_start_time = time.time()
-                if self.current_wp_index < len(self.waypoints):
-                    self.state = MissionState.NAVIGATING
-                else:
-                    # 【核心修改点】所有航点巡视完毕，根据参数决定去向
-                    if self.return_to_home:
-                        print("\n[完成] 所有航点均已巡视完毕！准备返航起始点。")
-                        self.state = MissionState.RETURNING
-                    else:
-                        print("\n[完成] 所有航点均已巡视完毕！[设置] 不执行返航，就地保持悬停。")
-                        self.state = MissionState.HOLD_FINAL
 
+        # 5. HOLD_TASK: 抵达巡点后的搜索悬停阶段
+        elif self.state == MissionState.HOLD_TASK:
+            # 优先判定视觉：如果在巡点悬停搜寻期间，视觉发现了目标
+            if target_detected:
+                print("\n🎯 [目标锁定] 视觉检测到目标！立刻中断悬停，切入 [视觉追踪] 状态！")
+                self.state = MissionState.VISUAL_TRACKING
+                self.tracking_start_time = time.time()
+                return
+
+            # 如果没有发现目标，则保持航点位置，并检查悬停是否超时
+            wp = self.waypoints[self.current_wp_index]
+            self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
+
+            elapsed_time = time.time() - self.hold_start_time
+            print(f"\r[检索中] 保持航点中... 剩余搜寻时间: {max(0.0, self.hold_duration - elapsed_time):.1f}s", end="",
+                  flush=True)
+
+            if elapsed_time >= self.hold_duration:
+                print(f"\n[未发现目标] 搜寻超时。放弃当前点，前往下一个航点...")
+                self.current_wp_index += 1
+                self.state = MissionState.NAVIGATING
+
+        # 6. VISUAL_TRACKING: 🎯 视觉闭环追踪状态（核心新增）
+        elif self.state == MissionState.VISUAL_TRACKING:
+            # 在此状态下，高层状态机【不主动发送】绝对位置坐标，把控制权完全让渡给主循环的 PID 速度控制
+            # 我们只需要在这里进行任务结束判定
+            print(f"\r[追踪中] 正在执行AI视觉闭环精准追踪... 计时: {time.time() - self.tracking_start_time:.1f}s",
+                  end="", flush=True)
+
+            if self._is_tracking_task_complete():
+                print("\n✅ [任务完成] 当前目标追踪任务圆满完成！重置追踪器，继续执行航点巡航...")
+                self.current_wp_index += 1  # 切换到下一个航点
+                self.state = MissionState.NAVIGATING
+
+        # 7. RETURNING: 自动返航至起始点上方
         elif self.state == MissionState.RETURNING:
-            # 飞回记录的 Home 起始点高度位置
+            self.drone.send_target_position(self.home_x, self.home_y, self.target_altitude, 0)
-            self.drone.send_target_position(self.home_x, self.home_y, self.home_z, 0)
 
             dist_to_home = math.sqrt((self.home_x - self.drone.local_position[0]) ** 2 +
                                      (self.home_y - self.drone.local_position[1]) ** 2)
@@ -163,25 +190,19 @@ class MissionManager:
                 else:
                     print("⚠️ 降落模式切换失败，正在重新下发...")
 
+        # 8. LANDING: 自动降落中
+        elif self.state == MissionState.LANDING:
+            print(f"\r[降落中] 当前高度: {self.drone.local_position[2]:+.2f}m | 解锁状态: {self.drone.is_armed}",
+                  end="", flush=True)
+            if not self.drone.is_armed:
+                print("\n[完结] 无人机已安全着陆并自动上锁。整个任务流结束。")
+                self.state = MissionState.FINISHED
+
+        # 9. HOLD_FINAL: 末端就地悬停
         elif self.state == MissionState.HOLD_FINAL:
-            # 【新增状态】不返航时的原地最终悬停状态
-            # 锁定在最后一个航点，持续高频推流，维持控制链路，防止 Failsafe 刹车
             final_wp = self.waypoints[-1] if self.waypoints else {'x': self.home_x, 'y': self.home_y,
                                                                   'z': self.target_altitude, 'yaw': 0}
             self.drone.send_target_position(final_wp['x'], final_wp['y'], final_wp['z'], final_wp['yaw'])
             print(
                 f"\r[挂起] 无人机保持在最终航点悬停。坐标:({final_wp['x']}, {final_wp['y']}, {final_wp['z']}) | 等待人工接管中...",
                 end="", flush=True)
-
-        elif self.state == MissionState.LANDING:
-            # 判断是否已经安全着陆并自动上锁
-            if not self.drone.is_armed:
-                print("\n[完成] 无人机已安全着陆并自动上锁，任务圆满结束！")
-                self.state = MissionState.FINISHED
-            else:
-                print("\r[降落中] 正在下降着陆中...", end="", flush=True)
-
-        elif self.state == MissionState.FINISHED:
-            return False
-
-        return True

gent_mission.py

@@ -0,0 +1,296 @@
+import os
+import cv2
+import numpy as np
+import time
+import threading
+import queue
+import socket
+import json
+import sys
+from ais_bench.infer.interface import InferSession
+
+# ================== 导入基础控制与状态机模块 =====================
+from drone_controller.base_control import DroneController
+from drone_controller.pid_counter import PID
+from drone_controller.mission_manager import MissionManager, MissionState, FailsafeTriggered
+# ===================================================================
+
+class VideoStreaming:
+    """ 📡 视频流采集与后台图传模块 """
+
+    def __init__(self, video_cfg, net_cfg):
+        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")
+        self.udp_port = net_cfg.get("udp_port", 9999)
+
+        self.cap = cv2.VideoCapture(self.video_source)
+        self.tx_queue = queue.Queue(maxsize=2)
+        self.is_running = True
+
+        if not self.cap.isOpened():
+            raise RuntimeError(f"❌ 无法打开视频源: {self.video_source}")
+
+        self.sender_thread = threading.Thread(target=self._udp_stream_sender_worker, daemon=True)
+        self.sender_thread.start()
+
+    def read_frame(self):
+        return self.cap.read()
+
+    def push_frame(self, frame):
+        if not self.tx_queue.full():
+            self.tx_queue.put(frame.copy())
+
+    def _udp_stream_sender_worker(self):
+        udp_socket = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+        dest_addr = (self.gs_ip, self.udp_port)
+        print(f"📡 [图传后台] UDP 推流服务已启动，目标地面站: {self.gs_ip}:{self.udp_port}")
+
+        while self.is_running:
+            try:
+                frame = self.tx_queue.get(timeout=1.0)
+                ret, jpeg_buf = cv2.imencode('.jpg', frame, [int(cv2.IMWRITE_JPEG_QUALITY), self.jpeg_quality])
+                if not ret:
+                    continue
+
+                data = jpeg_buf.tobytes()
+                packet_size = 60000
+                total_size = len(data)
+
+                for i in range(0, total_size, packet_size):
+                    udp_socket.sendto(data[i:i + packet_size], dest_addr)
+            except queue.Empty:
+                continue
+            except Exception as e:
+                print(f"⚠️ [图传异常] 发送失败: {e}")
+        udp_socket.close()
+
+    def release(self):
+        self.is_running = False
+        if self.cap.isOpened():
+            self.cap.release()
+
+
+class YOLO26UAVInfer:
+    """ 🧠 昇腾 NPU YOLOv8 极致推理模块 """
+
+    def __init__(self, model_cfg):
+        self.model_path = model_cfg.get("model_path", "./om/yolo26n-balloon.om")
+        self.conf_threshold = model_cfg.get("conf_threshold", 0.25)
+        device_id = model_cfg.get("device_id", 0)
+
+        print(f"🧠 [NPU 初始化] 正在载入昇腾 OM 模型: {self.model_path} ...")
+        self.session = InferSession(device_id, self.model_path)
+
+    def preprocess(self, img, img_size=640):
+        h, w = img.shape[:2]
+        r = min(img_size / h, img_size / w)
+        unpad_h, unpad_w = int(round(h * r)), int(round(w * r))
+
+        dw, dh = img_size - unpad_w, img_size - unpad_h
+        dw /= 2
+        dh /= 2
+
+        if (w, h) != (unpad_w, unpad_h):
+            img_resized = cv2.resize(img, (unpad_w, unpad_h), interpolation=cv2.INTER_LINEAR)
+        else:
+            img_resized = img
+
+        top, bottom = int(round(dh - 0.1)), int(round(dh + 0.1))
+        left, right = int(round(dw - 0.1)), int(round(dw + 0.1))
+        img_padded = cv2.copyMakeBorder(img_resized, top, bottom, left, right, cv2.BORDER_CONSTANT,
+                                        value=(114, 114, 114))
+
+        img_in = img_padded.transpose((2, 0, 1))[::-1]  # BGR to RGB
+        img_in = np.ascontiguousarray(img_in).astype(np.float32) / 255.0
+        return img_in, r, (dw, dh)
+
+    def postprocess(self, outputs, ratio, dwdh, orig_shape):
+        boxout = outputs[0][0]
+        box_num = outputs[1][0]
+
+        detections = []
+        for i in range(box_num):
+            box = boxout[i]
+            score = box[4]
+            cls_id = int(box[5])
+
+            if score < self.conf_threshold:
+                continue
+
+            x1, y1, x2, y2 = box[0], box[1], box[2], box[3]
+            x1 -= dwdh[0]
+            x2 -= dwdh[0]
+            y1 -= dwdh[1]
+            y2 -= dwdh[1]
+
+            x1, x2 = x1 / ratio, x2 / ratio
+            y1, y2 = y1 / ratio, y2 / ratio
+
+            x1 = max(0, min(x1, orig_shape[1]))
+            y1 = max(0, min(y1, orig_shape[0]))
+            x2 = max(0, min(x2, orig_shape[1]))
+            y2 = max(0, min(y2, orig_shape[0]))
+
+            detections.append({
+                "box": [int(x1), int(y1), int(x2), int(y2)],
+                "score": float(score),
+                "class": cls_id
+            })
+        return detections
+
+    def draw_boxes(self, frame, detections):
+        for det in detections:
+            x1, y1, x2, y2 = det["box"]
+            cv2.rectangle(frame, (x1, y1), (x2, y2), (0, 255, 0), 2)
+            cv2.putText(frame, f"Target: {det['score']:.2f}", (x1, y1 - 10),
+                        cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+        return frame
+
+
+class UAVControlLoop:
+    """ ✈️ 融合型无人机控制闭环中心（深度适配状态机） """
+
+    def __init__(self, flight_cfg, pid_y_cfg, pid_z_cfg):
+        self.uav = DroneController(
+            connection_string=flight_cfg.get("connection_string", "/dev/ttyUSB0"),
+            baud=flight_cfg.get("baud_rate", 57600)
+        )
+        self.pid_y = PID(**pid_y_cfg)
+        self.pid_z = PID(**pid_z_cfg)
+
+    def start_uav(self):
+        print("⚡ [底层通信] 正在建立与飞控的 MAVLink 链路...")
+        if self.uav.connect():
+            print("✅ [底层通信] 飞控监听守护线程就绪。")
+            return True
+        print("❌ [底层通信] 串口独占失败或未连上。")
+        return False
+
+    def process_control(self, detections, orig_shape, frame, mission_state):
+        """
+        根据当前状态机状态，决定是否发送闭环 PID 控制速度
+        """
+        h, w = orig_shape
+        center_x_target, center_y_target = w / 2.0, h / 2.0
+
+        # 绘制画面正中央的十字瞄准准心
+        cv2.drawMarker(frame, (int(center_x_target), int(center_y_target)), (0, 0, 255), cv2.MARKER_CROSS, 20, 2)
+
+        # 判定是否包含有效检测目标
+        if len(detections) > 0:
+            best_det = max(detections, key=lambda x: x["score"])
+            x1, y1, x2, y2 = best_det["box"]
+
+            # 计算目标的像素中心点
+            obj_x = (x1 + x2) / 2.0
+            obj_y = (y1 + y2) / 2.0
+
+            # 绘制目标质点追踪轨迹
+            cv2.circle(frame, (int(obj_x), int(obj_y)), 5, (255, 0, 0), -1)
+            cv2.line(frame, (int(center_x_target), int(center_y_target)), (int(obj_x), int(obj_y)), (255, 255, 0), 2)
+
+            # 归一化百分比误差计算值
+            err_x = (obj_x - center_x_target) / center_x_target
+            err_y = (obj_y - center_y_target) / center_y_target
+
+            # 核心控制逻辑决策：只有当状态机真正进入 VISUAL_TRACKING 状态，香橙派才拥有物理速度控制权！
+            if mission_state == MissionState.VISUAL_TRACKING:
+                vy_cmd = self.pid_y.update(err_x)
+                vz_cmd = self.pid_z.update(err_y)
+
+                # 下发机体系速度：前房保持0（悬停对准），vy左右平移，vz上下爬升
+                self.uav.send_body_velocity(vx=0.0, vy=vy_cmd, vz=vz_cmd, yaw_rate=0.0)
+        else:
+            # 目标丢失兜底保护：如果在追踪状态下目标跟丢了，立即刹车原地悬停，并清除积分项
+            if mission_state == MissionState.VISUAL_TRACKING:
+                self.uav.send_body_velocity(0, 0, 0, 0)
+                self.pid_y.reset()
+                self.pid_z.reset()
+
+
+def load_config(config_path="./config.json"):
+    with open(config_path, "r", encoding="utf-8") as f:
+        return json.load(f)
+
+
+if __name__ == "__main__":
+    # 模拟外部定义的自动化航点列表（NED坐标系：北, 东, 地。Z 轴向上为负，所以高度 1.5 米设为 -1.5）
+    test_waypoints = [
+        {'x': 1.0, 'y': 0.0, 'z': -1.5, 'yaw': 0},
+        {'x': 2.0, 'y': 1.5, 'z': -1.5, 'yaw': 45},
+        {'x': 0.0, 'y': 2.0, 'z': -1.5, 'yaw': 90}
+    ]
+
+    cfg = load_config()
+
+    # 实例化子系统
+    streamer = VideoStreaming(cfg["video"], cfg["network"])
+    detector = YOLO26UAVInfer(cfg["model"])
+    controller = UAVControlLoop(cfg["flight_control"], cfg["pid_y"], cfg["pid_z"])
+
+    # 启动无人机串口连接
+    if not controller.start_uav():
+        sys.exit("❌ 飞控连接失败，无法继续业务")
+
+    # 实例化复合状态机业务管理器，配置不发现目标时巡点悬停检索时间为 8 秒，飞完直接原地降落不返航
+    mission = MissionManager(
+        drone=controller.uav,
+        waypoints=test_waypoints,
+        target_altitude=1.5,
+        arrival_radius=0.3,
+        hold_duration=8.0,
+        return_to_home=False
+    )
+
+    print("🚀 NPU 精准推理 [航点巡航与视觉追踪融合版] 主业务循环全面启动...")
+
+    try:
+        while True:
+            ret, frame = streamer.read_frame()
+            if not ret:
+                print("❌ 未能读取到摄像头数据，退出中...")
+                break
+
+            orig_shape = frame.shape[:2]
+
+            # 1. 图像预处理与硬件 NPU 高速推理
+            input_tensor, ratio, dwdh = detector.preprocess(frame)
+            outputs = detector.session.infer([input_tensor])
+
+            # 2. 坐标反求与后处理
+            detections = detector.postprocess(outputs, ratio, dwdh, orig_shape)
+
+            # 判断当前帧是否捕捉到了目标
+            target_detected = len(detections) > 0
+
+            # 3. 驱动高层任务管理器状态机（传入视觉反馈）
+            try:
+                mission.update(target_detected=target_detected)
+            except FailsafeTriggered:
+                # 触发人肉拦截，跳出死循环，优雅关闭程序
+                break
+
+            # 4. 驱动底层视觉闭环速度计算（只有状态机进入 VISUAL_TRACKING 时才会真正发送速度指令）
+            controller.process_control(detections, orig_shape, frame, mission.state)
+
+            # 5. 地面站视频帧渲染绘制
+            frame = detector.draw_boxes(frame, detections)
+
+            # 在图传画面上实时叠加当前系统状态机状态
+            status_text = f"State: {mission.state.name} | WP: {mission.current_wp_index + 1}/{len(test_waypoints)}"
+            cv2.putText(frame, status_text, (20, 40), cv2.FONT_HERSHEY_COMPLEX, 0.6, (0, 255, 255), 2)
+
+            # 推送视频到后台队列传输至 PC
+            streamer.push_frame(frame)
+
+            # 限制主循环高频速度（30Hz 左右），防止高频过度占用 CPU
+            time.sleep(0.033)
+
+    except KeyboardInterrupt:
+        print("\n👋 接收到终端键盘中断指令，正在安全退出...")
+    finally:
+        # 优雅释放
+        streamer.release()
+        controller.uav.close()
+        print("🏁 系统资源已安全回收，程序退出。")