From c6fc0dd775b47b4ddb2b4650a6c8cb340e7c50a4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?=E4=B8=80=E5=96=B5?= Date: Tue, 2 Jun 2026 11:03:37 +0800 Subject: [PATCH] =?UTF-8?q?=E9=83=A8=E5=88=86=E9=87=8D=E6=9E=84=E4=B8=8E?= =?UTF-8?q?=E8=87=AA=E5=8A=A8=E5=B7=A1=E9=A3=9E?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 4 + README.md | 10 +- check.py | 2 +- config.json | 33 ++++ drone_controller/base_control.py | 44 ++++- drone_controller/mission_manager.py | 145 ++++++++++++++ infer_camera_modular.py | 293 ++++++++++++++++++++++++++++ infer_video_direct.py | 2 +- receiver.py | 2 +- run_mission.py | 59 ++++++ 10 files changed, 587 insertions(+), 7 deletions(-) create mode 100644 .vscode/settings.json create mode 100644 config.json create mode 100644 drone_controller/mission_manager.py create mode 100644 infer_camera_modular.py create mode 100644 run_mission.py diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 0000000..4b5a294 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,4 @@ +{ + "python-envs.defaultEnvManager": "ms-python.python:conda", + "python-envs.defaultPackageManager": "ms-python.python:conda" +} \ No newline at end of file diff --git a/README.md b/README.md index 7bbb3f8..d17a75e 100644 --- a/README.md +++ b/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:控制模块 \ No newline at end of file diff --git a/check.py b/check.py index fad0073..1bb84dd 100644 --- a/check.py +++ b/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}") diff --git a/config.json b/config.json new file mode 100644 index 0000000..6724c8d --- /dev/null +++ b/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 + } +} \ No newline at end of file diff --git a/drone_controller/base_control.py b/drone_controller/base_control.py index 51c2a1d..64ecbc8 100644 --- a/drone_controller/base_control.py +++ b/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底层控制接口已安全断开。") \ No newline at end of file diff --git a/drone_controller/mission_manager.py b/drone_controller/mission_manager.py new file mode 100644 index 0000000..09ced03 --- /dev/null +++ b/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 \ No newline at end of file diff --git a/infer_camera_modular.py b/infer_camera_modular.py new file mode 100644 index 0000000..a2a2d43 --- /dev/null +++ b/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() \ No newline at end of file diff --git a/infer_video_direct.py b/infer_video_direct.py index e420f04..0e9dfe0 100644 --- a/infer_video_direct.py +++ b/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 # 置信度阈值 # -------------- 无线图传网络配置 -------------- diff --git a/receiver.py b/receiver.py index 3ddbb87..2d0884f 100644 --- a/receiver.py +++ b/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 # 录像固定的标准帧率 # ======================================================= diff --git a/run_mission.py b/run_mission.py new file mode 100644 index 0000000..30cc987 --- /dev/null +++ b/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() \ No newline at end of file