部分重构与自动巡飞

.vscode/settings.json

@@ -0,0 +1,4 @@
+{
+    "python-envs.defaultEnvManager": "ms-python.python:conda",
+    "python-envs.defaultPackageManager": "ms-python.python:conda"
+}

README.md

@@ -2,16 +2,20 @@
 
 这些会部署在香橙派AI Pro上，模型为*YOLOv26n*
 
-说明下各文件的作用
+### 说明下各文件的作用
 - check.py:确认输入视频帧率，用于性能测试的;
 - control.py:PID计算和MAVLink指令发送方法的定义(已废弃);
 - fc_test:获取飞控信息的测试脚本;
 - infer_camera_direct.py:通过摄像头检测目标并调用方法控制无人机;
 - infer_camera_video.py:使用视频检测目标并调用方法控制无人机(用于测试);
 - infer.py:使用图片检测目标，仅用于测试环境;
+#### 无人机控制部分:
 - base_control.py:定义了部分基本动作的方法
+#### 视觉跟踪业务:
 - pid_counter.py:抽离出来的PID计算器
-
+#### 自动巡点飞行业务:
-目录文件：
+- mission_manager.py:自动巡点飞行的控制模块
+- run_mission.py:自动巡点飞行的业务测试
+#### 目录：
 - om:推理使用的.om文件
 - drone_controller:控制模块

check.py

@@ -4,7 +4,7 @@
 import cv2
 import os
 
-video_path = "saved_video/output_result.mp4"  # 替换成你上传的视频文件名
+video_path = "asset/saved_video/output_result.mp4"  # 替换成你上传的视频文件名
 
 if not os.path.exists(video_path):
     print(f"错误：找不到文件 {video_path}")

config.json

@@ -0,0 +1,33 @@
+{
+    "model": {
+        "model_path": "./om/yolo26n-balloon.om",
+        "conf_threshold": 0.25,
+        "device_id": 0
+    },
+    "video": {
+        "video_source": 0,
+        "jpeg_quality": 75
+    },
+    "network": {
+        "ground_station_ip": "192.168.31.239",
+        "udp_port": 9999
+    },
+    "flight_control": {
+        "connection_string": "/dev/ttyUSB0",
+        "baud_rate": 57600
+    },
+    "pid_y": {
+        "kp": 1.0,
+        "ki": 0.02,
+        "kd": 0.3,
+        "max_out": 0.8,
+        "min_out": -0.8
+    },
+    "pid_z": {
+        "kp": 1.0,
+        "ki": 0.02,
+        "kd": 0.3,
+        "max_out": 0.4,
+        "min_out": -0.4
+    }
+}

drone_controller/base_control.py

@@ -170,9 +170,51 @@ class DroneController:
             end="", flush=True
         )
 
+    def takeoff(self, altitude):
+        """发送起飞指令(仅在 GUIDED 或 OFFBOARD 模式下有效)"""
+        if not self.is_connected: return False
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_NAV_TAKEOFF,
+            0, 0, 0, 0, 0, 0, 0, altitude
+        )
+        print(f"已下发自动起飞指令 -> 目标高度: {altitude} 米")
+        return True
+
+    def send_target_position(self, x, y, z, yaw):
+        """
+        发送绝对局部位置( NED 坐标系)
+        x:北向目标位置
+        y:东向目标位置
+        z:垂直目标位置
+        """
+        if not self.is_connected: return
+        # 当人为切出切换模式时，香橙派不再发送命令
+        if self.current_mode not in ['GUIDED', 'OFFBOARD']: return
+
+        # 使用局部 NED 坐标系
+        coordinate_frame = mavutil.mavlink.MAV_FRAME_NED
+
+        # 掩码屏蔽:只保留位置 (x, y, z) 和偏航角 (yaw)
+        type_mask = 0b0000101111111000
+
+        self.master.mav.set_position_target_local_ned_send(
+            0,
+            self.master.target_system,
+            self.master.target_component,
+            coordinate_frame,
+            type_mask,
+            x, y, z,
+            0, 0, 0,
+            0, 0, 0,
+            math.radians(yaw),
+            0
+        )
+
     def close(self):
         """释放线程，关闭串口连接"""
         self.running = False
         if self.telemetry_thread:
-            self.telemetry_thread.join(timeout=1)
+            self.telemetry_thread.join(timeout=1.0)
         print("\n底层控制接口已安全断开。")

drone_controller/mission_manager.py

@@ -0,0 +1,145 @@
+import time
+import math
+from enum import Enum
+
+
+class MissionState(Enum):
+    INIT = 0        # 初始化/等待连接
+    ARMING = 1      # 解锁中
+    TAKEOFF = 2     # 自动起飞中
+    NAVIGATING = 3  # 航点巡航导航
+    HOLD_TASK = 4   # 到达航点后的原地任务悬停
+    RETURNING = 5   # 返航或直接准备降落
+    LANDING = 6     # 自动降落中
+    FINISHED = 7    # 任务结束/已上锁
+
+
+class MissionManager:
+    def __init__(self, drone, waypoints, target_altitude=1.5, arrival_radius=0.3, hold_duration=5.0):
+        """
+        :param drone: 实例化的 DroneController 底层驱动对象
+        :param waypoints: 航点列表，格式为 [{'x': 1.0, 'y': 2.0, 'z': -1.5, 'yaw': 0}, ...]
+                          注意：NED 坐标系中，Z轴向上为负，所以起飞或目标高度应为负值
+        :param target_altitude: 初始起飞高度（米，正数，内部会自动转为NED负值）
+        :param arrival_radius: 判定航点到达的三维欧氏距离阈值（米）
+        :param hold_duration: 在每个航点到达后，执行任务（或悬停）的停留时间（秒）
+        """
+        self.drone = drone
+        self.waypoints = waypoints
+        self.target_altitude = -abs(target_altitude)  # 强制转为 NED 坐标系的负高度
+        self.arrival_radius = arrival_radius
+        self.hold_duration = hold_duration
+
+        self.state = MissionState.INIT
+        self.current_wp_index = 0
+        self.hold_start_time = None
+
+    def calculate_distance(self, target_x, target_y, target_z):
+        """计算无人机当前局部位置与目标点之间的三维欧氏距离"""
+        dx = target_x - self.drone.local_x
+        dy = target_y - self.drone.local_y
+        dz = target_z - self.drone.local_z
+        return math.sqrt(dx ** 2 + dy ** 2 + dz ** 2)
+
+    def update(self):
+        """
+        核心状态机轮询函数。
+        高层主循环高频调用此函数，根据当前状态驱动飞行任务。
+        """
+        # 安全人肉兜底：如果安全员从遥控器切出 GUIDED 模式，状态机挂起，不执行任何行为
+        if self.state not in [MissionState.INIT, MissionState.ARMING]:
+            if self.drone.current_mode not in ['GUIDED', 'OFFBOARD']:
+                print(f"\r\n⚠️  [警告] 安全员切出控制模式！当前模式为: {self.drone.current_mode}, 任务管理程序挂起...", end="")
+                return False
+
+        # ----------------- 状态机逻辑 -----------------
+        if self.state == MissionState.INIT:
+            if self.drone.is_connected:
+                print("\n[状态] 飞控通信已建立，准备切换至 GUIDED 模式并解锁...")
+                if self.drone.set_mode('GUIDED'):   #若为 PX4 则是OFFBOARD
+                    self.state = MissionState.ARMING
+            else:
+                print("\r[状态] 等待飞控心跳包连接...", end="", flush=True)
+
+        elif self.state == MissionState.ARMING:
+            print("[状态] 正在发送解锁指令...")
+            if self.drone.arm():
+                print("[成功] 飞控解锁成功，正在下发自动起飞指令...")
+                # 转换到起飞高度，注意发送的是相对高度或绝对位置
+                if self.drone.takeoff(abs(self.target_altitude)):
+                    self.state = MissionState.TAKEOFF
+            else:
+                print("[重试] 解锁未被飞控确认，1秒后重新尝试解锁...")
+                time.sleep(1)
+
+        elif self.state == MissionState.TAKEOFF:
+            # 判定起飞是否到达指定高度（NED 坐标系下，比较 z 值）
+            dist_to_takeoff = abs(self.drone.local_z - self.target_altitude)
+            print(
+                f"\r[起飞中] 当前高度: {-self.drone.local_z:.2f}M -> 目标: {abs(self.target_altitude)}M | 误差: {dist_to_takeoff:.2f}M",
+                end="", flush=True)
+
+            if dist_to_takeoff <= self.arrival_radius:
+                print(f"\n[成功] 已到达起飞高度！开始执行巡点导航，共有 {len(self.waypoints)} 个航点。")
+                if len(self.waypoints) > 0:
+                    self.current_wp_index = 0
+                    self.state = MissionState.NAVIGATING
+                else:
+                    self.state = MissionState.RETURNING
+
+        elif self.state == MissionState.NAVIGATING:
+            wp = self.waypoints[self.current_wp_index]
+            # 持续高频向飞控下发当前期望航点
+            self.drone.send_target_position(wp['x'], wp['y'], wp['z'], wp['yaw'])
+
+            # 计算距离
+            dist = self.calculate_distance(wp['x'], wp['y'], wp['z'])
+            print(
+                f"\r[巡航中] 正在飞往航点 [{self.current_wp_index + 1}]: 目标({wp['x']}, {wp['y']}, {wp['z']}) | 剩余距离: {dist:.2f}米",
+                end="", flush=True)
+
+            # 到达判定
+            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",
+                end="", flush=True)
+
+            if elapsed >= self.hold_duration:
+                print(f"\n[完成] 航点 [{self.current_wp_index + 1}] 任务执行完毕。")
+                # 切换下一个点
+                self.current_wp_index += 1
+                if self.current_wp_index < len(self.waypoints):
+                    self.state = MissionState.NAVIGATING
+                else:
+                    print("\n[完成] 所有航点均已巡视完毕！准备返航/降落。")
+                    self.state = MissionState.RETURNING
+
+        elif self.state == MissionState.RETURNING:
+            print("\n[降落中] 正在下发自动降落指令 (LAND)...")
+            if self.drone.set_mode('LAND'):
+                self.state = MissionState.LANDING
+            else:
+                print("⚠️ 降落模式切换失败，正在重新下发...")
+
+        elif self.state == MissionState.LANDING:
+            # 判断是否已经安全着陆并自动上锁
+            if not self.drone.is_armed:
+                print("\n[成功] 检测到无人机已安全着陆并自动上锁。任务圆满结束！")
+                self.state = MissionState.FINISHED
+            else:
+                print(f"\r[降落中] 正在着陆... 当前高度: {-self.drone.local_z:.2f}M", end="", flush=True)
+
+        elif self.state == MissionState.FINISHED:
+            return True  # 任务全部结束
+
+        return False

infer_camera_modular.py

@@ -0,0 +1,293 @@
+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
+# ===================================================
+
+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_to_stream(self, frame):
+        """ 非阻塞式推入图传队列 """
+        try:
+            # 推送前轻量化分辨率，降低带宽压力
+            send_frame = cv2.resize(frame, (640, 480))
+            self.tx_queue.put_nowait(send_frame)
+        except queue.Full:
+            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}")
+
+        while self.is_running:
+            try:
+                frame_to_send = self.tx_queue.get(timeout=1.0)
+                if frame_to_send is None:
+                    break
+
+                result, img_encode = cv2.imencode('.jpg', frame_to_send, [cv2.IMWRITE_JPEG_QUALITY, self.jpeg_quality])
+                if not result:
+                    continue
+
+                data = img_encode.tobytes()
+                if len(data) > 65000:  # 严格防丢包限制
+                    continue
+
+                sock.sendto(data, server_address)
+            except queue.Empty:
+                continue
+            except Exception as e:
+                print(f"❌ 图传线程异常: {e}")
+                time.sleep(0.1)
+
+        sock.close()
+
+    def release(self):
+        """ 释放资源 """
+        self.is_running = False
+        self.tx_queue.put(None)
+        self.cap.release()
+
+
+class YOLO26UAVInfer:
+    """ 🚀 昇腾 NPU 推理与图像后处理模块 """
+
+    def __init__(self, model_cfg):
+        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)
+
+        if not os.path.exists(model_path):
+            raise FileNotFoundError(f"❌ 找不到模型文件: {model_path}")
+
+        self.session = InferSession(device_id, model_path)
+        self.input_width = 640
+        self.input_height = 640
+
+    def letterbox(self, img, new_shape=(640, 640), color=(114, 114, 114)):
+        shape = img.shape[:2]  # [height, width]
+        r = min(new_shape[0] / shape[0], new_shape[1] / shape[1])
+        new_unpad = (int(round(shape[1] * r)), int(round(shape[0] * r)))
+        dw, dh = new_shape[1] - new_unpad[0], new_shape[0] - new_unpad[1]
+        dw /= 2
+        dh /= 2
+        if shape[::-1] != new_unpad:
+            img = cv2.resize(img, new_unpad, interpolation=cv2.INTER_LINEAR)
+        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 = cv2.copyMakeBorder(img, top, bottom, left, right, cv2.BORDER_CONSTANT, value=color)
+        return img, r, (dw, dh)
+
+    def preprocess(self, frame):
+        """ 图像预处理，输出符合 NPU 输入的 Tensor """
+        input_tensor, ratio, dwdh = self.letterbox(frame)
+        input_tensor = input_tensor.astype(np.float32) / 255.0
+        input_tensor = np.transpose(input_tensor, (2, 0, 1))
+        input_tensor = np.expand_dims(input_tensor, axis=0)
+        input_tensor = np.ascontiguousarray(input_tensor)
+        return input_tensor, ratio, dwdh
+
+    def postprocess(self, outputs, ratio, dwdh, orig_shape):
+        out = outputs[0]
+        detections = []
+        for i in range(out.shape[1]):
+            box = out[0, i, :4]
+            conf = out[0, i, 4]
+            cls_id = int(out[0, i, 5])
+            if conf > self.conf_threshold:
+                x1, y1, x2, y2 = box
+                # 基于原图比例还原坐标
+                x1 = (x1 - dwdh[0]) / ratio
+                y1 = (y1 - dwdh[1]) / ratio
+                x2 = (x2 - dwdh[0]) / ratio
+                y2 = (y2 - dwdh[1]) / 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([x1, y1, x2, y2, conf, cls_id])
+        return detections
+
+    def draw_boxes(self, img, detections):
+        """ 在图像上渲染精准目标框 """
+        for det in detections:
+            x1, y1, x2, y2, conf, cls_id = det
+            cv2.rectangle(img, (int(x1), int(y1)), (int(x2), int(y2)), (0, 255, 0), 2)
+            label = f"Target: {conf:.2f}"
+            cv2.putText(img, label, (int(x1), int(y1) - 10), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0, 255, 0), 2)
+        return img
+
+
+class UAVControlLoop:
+    """ 🛸 无人机飞控闭环控制核心 """
+
+    def __init__(self, fc_cfg, pid_y_cfg, pid_z_cfg):
+        connection_string = fc_cfg.get("connection_string", "/dev/ttyUSB0")
+        baud_rate = fc_cfg.get("baud_rate", 57600)
+
+        # 初始化控制驱动端
+        self.uav = DroneController(connection_string=connection_string, baud=baud_rate)
+
+        # 从 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"]
+        )
+
+    def start_uav(self):
+        """ 连接并解锁无人机 """
+        self.uav.connect()
+        if not self.uav.is_armed and self.uav.current_mode not in ['GUIDED', 'OFFBOARD']:
+            self.uav.arm()
+
+    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')
+
+        if len(detections) > 0:
+            # 寻找置信度最高的目标
+            best_det = max(detections, key=lambda x: x[4])
+            x1, y1, x2, y2, conf, cls_id = best_det
+            target_x = (x1 + x2) / 2
+            target_y = (y1 + y2) / 2
+
+            # 计算归一化误差
+            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
+
+            # 视效叠加
+            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)
+
+            # 执行机体速度控制
+            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)
+
+            # 清除 PID 积分，防止突跳
+            if hasattr(self.pid_y, 'reset'): self.pid_y.reset()
+            if hasattr(self.pid_z, 'reset'): self.pid_z.reset()
+
+            if current_mode in ['GUIDED', 'OFFBOARD']:
+                self.uav.send_body_velocity(vx=0.0, vy=0.0, vz=0.0, yaw_rate=0.0)
+
+
+# ==================== 主程序入口 ======================
+def main():
+    # 默认加载同目录下的 uav_config.json
+    config_file = "uav_config.json"
+
+    # 进阶功能：允许在命令行指定 json 路径，例如: python3 infer_camera_modular.py config_outdoor.json
+    if len(sys.argv) > 1:
+        config_file = sys.argv[1]
+
+    print(f"📖 正在从 {config_file} 加载系统配置...")
+    try:
+        with open(config_file, "r", encoding="utf-8") as f:
+            cfg = json.load(f)
+    except Exception as e:
+        print(f"❌ 读取配置文件失败: {e}")
+        sys.exit(1)
+
+    # 实例化各子系统并注入配置
+    streamer = VideoStreaming(cfg["video"], cfg["network"])
+    detector = YOLO26UAVInfer(cfg["model"])
+    controller = UAVControlLoop(cfg["flight_control"], cfg["pid_y"], cfg["pid_z"])
+
+    # 启动无人机连接
+    controller.start_uav()
+
+    print("🚀 NPU 精准推理 [JSON配置动态加载版] 主循环启动...")
+
+    try:
+        while True:
+            ret, frame = streamer.read_frame()
+            if not ret:
+                print("❌ 未能读取到摄像头数据，退出中...")
+                break
+
+            orig_shape = frame.shape[:2]
+
+            # 1. 图像预处理
+            input_tensor, ratio, dwdh = detector.preprocess(frame)
+
+            # 2. NPU 硬件推理
+            t_start = time.time()
+            outputs = detector.session.infer([input_tensor])
+            t_end = time.time()
+
+            fps_pure = 1.0 / (t_end - t_start)
+
+            # 3. 坐标反求与控制计算
+            detections = detector.postprocess(outputs, ratio, dwdh, orig_shape)
+            controller.process_control(detections, orig_shape, frame)
+
+            # 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)
+
+            # 5. 推送至后台异步图传队列
+            streamer.push_to_stream(frame)
+
+    except KeyboardInterrupt:
+        print("\n👋 接收到终止信号，安全退出中...")
+    finally:
+        streamer.release()
+        print("程序运行结束。")
+
+
+if __name__ == "__main__":
+    main()

infer_video_direct.py

@@ -9,7 +9,7 @@ from ais_bench.infer.interface import InferSession
 
 # ==================== 配置区域 ======================
 MODEL_PATH = "./om/yolo26n-balloon.om"          # YOLO26 模型路径
-INPUT_VIDEO = "./test_video.mp4"                # 输入的测试视频
+INPUT_VIDEO = "./asset/test_video.mp4"          # 输入的测试视频
 CONF_THRESHOLD = 0.25                           # 置信度阈值
 
 # -------------- 无线图传网络配置 --------------

receiver.py

@@ -8,7 +8,7 @@ import os
 
 # ==================== 地面站核心配置 ====================
 PORT = 9999
-SAVE_DIR = "./saved_video"
+SAVE_DIR = "./asset/saved_video"
 SAVE_PATH = os.path.join(SAVE_DIR, "ground_station_record.mp4")
 RECORD_FPS = 24.0  # 录像固定的标准帧率
 # =======================================================

run_mission.py

@@ -0,0 +1,59 @@
+# run_mission.py
+import time
+from drone_controller.base_control import DroneController
+from drone_controller.mission_manager import MissionManager
+
+def main():
+    # 1. 实例化飞控底层驱动 SDK
+    # 注意：根据现场情况修改你的串口和波特率
+    drone = DroneController(connection_string='/dev/ttyUSB0', baud=115200)
+    
+    try:
+        # 启动底层数据链连接与异步状态遥测线程
+        drone.connect()
+        
+        # 2. 规划室内巡点航点列表 (NED 相对局部坐标系)
+        # 规则说明：X-北为正, Y-东为正, Z-向下为正（所以想要在 1.5 米高度飞，Z 必须为 -1.5）
+        # yaw：偏航角角度(0-360度)，0代表正北
+        my_waypoints = [
+            {'x': 1.0,  'y': 0.0,  'z': -1.5, 'yaw': 0},   # 点1：向正前方推进1米
+            {'x': 1.0,  'y': 1.0,  'z': -1.5, 'yaw': 90},  # 点2：保持前推，向右平移1米，机头偏向正东
+            {'x': 0.0,  'y': 1.0,  'z': -1.5, 'yaw': 180}, # 点3：倒飞回起点水平线，机头朝向正南
+            {'x': 0.0,  'y': 0.0,  'z': -1.5, 'yaw': 270}  # 点4：返回起飞原点，机头朝向正西
+        ]
+        
+        # 3. 初始化任务管理器模块
+        mission = MissionManager(
+            drone=drone,
+            waypoints=my_waypoints,
+            target_altitude=1.5,    # 自动起飞高度 1.5 米
+            arrival_radius=0.25,    # 判定到点的欧氏距离误差阈值锁定在 25 厘米以内
+            hold_duration=4.0       # 每个点到达后悬停停留 4 秒
+        )
+        
+        print("\n================ 自动巡点系统就绪 ================")
+        print("提示: 现场安全员请时刻保持遥控器手柄在握，一旦发生异常，切出控制模式即可随时越权人工接管。")
+        print("================================================\n")
+        
+        # 4. 高频业务轮询主循环
+        while True:
+            # 驱动任务状态机
+            is_mission_finished = mission.update()
+            
+            if is_mission_finished:
+                print("\n[通知] 主程序检测到巡航任务模块已正常运行结束，退出程序。")
+                break
+                
+            # 20Hz 的控制频率（50ms一拍），既能保证位置控制实时下发，又不会拖垮处理器
+            time.sleep(0.05)
+            
+    except KeyboardInterrupt:
+        print("\n🛑 用户手动中止程序 (Ctrl+C)！正在触发紧急安全退出。")
+    finally:
+        # 5. 清理与释放资源
+        print("[清理] 正在关闭后台遥测数据链线程...")
+        drone.disconnect()
+        print("[清理] 程序已安全解脱。")
+
+if __name__ == '__main__':
+    main()