diff --git a/control.py b/control.py new file mode 100644 index 0000000..e7f07c6 --- /dev/null +++ b/control.py @@ -0,0 +1,103 @@ +import time +from pymavlink import mavutil + +class UAVController: + def __init__(self, port='/dev/ttyUSB0', baud=115200): + """初始化与飞控的 MAVLink 串口连接""" + try: + self.master = mavutil.mavlink_connection(port, baud=baud) + print(f" [飞控通信] 已连接串口{port}, 请等待心跳包...") + self.master.wait_heartbeat() + print(" [飞控通信] 收到飞控心跳包, 链接正常") + except Exception as e: + print(f" [飞控通信] 连接失败:{e}, 请检查") + + # =================== PID 参数 =================== + # 此时参数设定，err_x: 无人机的左右平移, err_y: 无人机的上下平移 (后续还要修改) + # =============================================== + self.kp = 1.2 # 响应强度 + self.ki = 0.02 # 消除风偏 + self.kd = 0.3 # 刹车减震 + + # 内部缓存 + self.last_err_x = 0.0 + self.last_err_y = 0.0 + self.integral_x = 0.0 + self.integral_y = 0.0 + self.last_time = time.time() + + def update_and_send_command(self, err_x, err_y): + """通过输入图像误差, 计算 PID 并通过 MAVLink 发送指令""" + if self.master is None: + return + + current_time = time.time() + dt = current_time - self.last_time + if dt <=0: dt = 0.02 #防止除以0 + + # ------------ PID 计算 ------------- + # 1. 比例项 (p) + p_out_x = self.kp * err_x + p_out_y = self.ki * err_y + + # 2. 积分项 (I) + self.integral_x = max(-0.3, min(self.integral_x + err_x * dt, 0.3)) + self.integral_y = max(-0.3, min(self.integral_y + err_y * dt, 0.3)) + i_out_x = self.ki * self.integral_x + i_out_y = self.kd * self.integral_y + + # 3. 微分项 (D) + d_out_x = self.kp * (err_x - self.last_err_x) / dt + d_out_y = self.kp * (err_y - self.last_err_y) / dt + + # 4. 总输出 (映射为物理速度: m/s) + vel_x_cmd = p_out_x + i_out_x + d_out_x + vel_y_cmd = p_out_y + i_out_y + d_out_y + + # ------------- 安全限速 -------------- + # 限制下速度，怕测试时飞太快 + MAX_VEL = 1.0 + vel_x_cmd = max(min(MAX_VEL, vel_x_cmd), -MAX_VEL) + vel_y_cmd = max(min(MAX_VEL, vel_y_cmd), -MAX_VEL) + + # 更新缓存 + self.last_err_x = err_x + self.last_err_y = err_y + self.last_time = current_time + + # ------------- 坐标系映射 ------------- + # 图像 X 轴正方向对应无人机机体坐标系的右侧平移速度 (vy) + # 图像 Y 轴正方向对应无人机(此时对应前视相机)下降速度 (vz) + vy = vel_x_cmd # 图像右侧，往右飞 + vz = vel_y_cmd * 1.0# 图像下方，往下飞, (乘1.0表示向下为正) + vx = 0.5 # 无人机以 0.5 m/s 持续前进 + + # ---------- 发送 MAVLink 指令 -------- + # 飞控处于 OFFBOARD (PX4) 模式才能响应此指令 + self.master.mav.set_position_target_local_ned_send( + 0, # 系统启动时间戳（填 0 即可） + self.master.target_system, # 目标系统 ID（自动获取） + self.master.target_component, # 目标组件 ID（自动获取） + mavutil.mavlink.MAV_FRAME_BODY_OFFSET_NED, # 选择“机体坐标系”（以无人机当前朝向为基准） + 0b0000111111000111, # 位掩码控制：二进制代表“忽略位置和加速度，只控制速度” + 0, 0, 0, # 位置 X, Y, Z (被掩码忽略) + vx, vy, vz, # 实际控制物理速度 (单位: 米/秒): 前进, 右移, 下降 + 0, 0, 0, # 加速度 (被掩码忽略) + 0, 0 # 偏航角, 偏航角速度 (被掩码忽略) + ) + print(f" MAVLink -> 前进:{vx:.2f} m/s, 右移:{vy:.2f} m/s, 下降:{vz:.2f} m/s") + + def send_hover_cmd(self): + """ 目标丢失保护: 发送指令, 让无人机原地悬停""" + if self.master is None: return + self.master.mav.set_position_target_local_ned_send( + 0, + self.master.target_system, + self.master.target_component, + 0b0000111111000111, + 0, 0, 0, + 0.0, 0.0, 0.0, + 0, 0, 0, + 0, 0 + ) + print("[安全兜底] 目标跟丢或未准许控制！已向飞控发送原地刹车悬停指令。") diff --git a/infer_camera_direct.py b/infer_camera_direct.py index f3e8e19..fb2d575 100644 --- a/infer_camera_direct.py +++ b/infer_camera_direct.py @@ -7,15 +7,15 @@ import queue import socket from ais_bench.infer.interface import InferSession -# ==================== 配置区域 ==================== -MODEL_PATH = "./om/yolo26n-balloon.om" # YOLO26 模型路径 -INPUT_VIDEO = "./test_video.mp4" # 输入的测试视频（或 0 代表摄像头） -CONF_THRESHOLD = 0.25 # 置信度阈值 +# ==================== 配置区域 ====================== +MODEL_PATH = "./om/yolo26n-balloon.om" # YOLO26 模型路径 +INPUT_VIDEO = "./test_video.mp4" # 输入的测试视频（或 0 代表摄像头） +CONF_THRESHOLD = 0.25 # 置信度阈值 # -------------- 无线图传网络配置 -------------- -GROUND_STATION_IP = "192.168.31.239" # 地面站的局域网 IP +GROUND_STATION_IP = "192.168.31.239" # 地面站的局域网 IP UDP_PORT = 9999 -JPEG_QUALITY = 75 # JPEG 压缩质量 (0-100) +JPEG_QUALITY = 75 # JPEG 压缩质量 (0-100) # ================================================== # 限制队列大小，发不完就直接丢帧，保证飞控算法实时性 @@ -108,6 +108,14 @@ def main(): detector = YOLO26UAVInfer(MODEL_PATH) cap = cv2.VideoCapture(INPUT_VIDEO) + from control import UAVController + + # 初始化飞控通信类 (后续需根据实际串口修改, 这里临时选用 /dev/ttyUSB0代替) + uav_controller = UAVController(port='/dev/ttyUSB0', baud=115200) + + # 控制总开关: 只有当人在地面发送允许指令才会开启 AI 追踪 + AI_CONTROL_ENABLE = False + if not cap.isOpened(): print("❌ 无法打开视频源") return @@ -122,7 +130,7 @@ def main(): if not ret: break - # 【核心修正】完全保留读取到的原图大小，坚决不在这一步进行 cv2.resize！ + # 完全保留读取到的原图大小 orig_shape = frame.shape[:2] img_center_x = orig_shape[1] / 2 img_center_y = orig_shape[0] / 2 diff --git a/receiver.py b/receiver.py index 3688fe3..3ddbb87 100644 --- a/receiver.py +++ b/receiver.py @@ -1,34 +1,142 @@ import socket import cv2 import numpy as np +import threading +import queue +import time +import os +# ==================== 地面站核心配置 ==================== PORT = 9999 -sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) -sock.bind(('0.0.0.0', PORT)) +SAVE_DIR = "./saved_video" +SAVE_PATH = os.path.join(SAVE_DIR, "ground_station_record.mp4") +RECORD_FPS = 24.0 # 录像固定的标准帧率 +# ======================================================= -print("📡 [笔记本地面站] 已启动，正在等待香橙派连线...") +# 确保保存视频的文件夹存在 +if not os.path.exists(SAVE_DIR): + os.makedirs(SAVE_DIR) -# 初始化地面站的强悍 CPU 录像（对应发送端压缩后的分辨率） -fourcc = cv2.VideoWriter_fourcc(*'mp4v') -video_writer = cv2.VideoWriter("./saved_video/ground_station_record.mp4", fourcc, 24.0, (640, 480)) +# 核心线程共享：最新的一帧画面与互斥锁（防止读写冲突） +latest_frame = None +frame_lock = threading.Lock() +is_running = True -try: - while True: - packet, _ = sock.recvfrom(65535) - data = np.frombuffer(packet, dtype=np.uint8) - frame = cv2.imdecode(data, cv2.IMREAD_COLOR) - if frame is not None: - # 1. 地面站录像 - video_writer.write(frame) +def udp_receiver_thread(): + """ 📡 后台极速收包线程 """ + global latest_frame, is_running + sock = socket.socket(socket.AF_INET, socket.SOCK_DGRAM) + # 放大系统内核缓冲区到 4MB，全力应对图传电台的无线网络抖动 + sock.setsockopt(socket.SOL_SOCKET, socket.SO_RCVBUF, 4 * 1024 * 1024) - # 2. 实时显示监控 - cv2.imshow("🚀 UAV AI Ground Station Monitor", frame) + try: + sock.bind(('0.0.0.0', PORT)) + print(f"📡 [地面站接收端] 已启动，正在监听 UDP 端口 {PORT}...") + except Exception as e: + print(f"❌ 端口绑定失败: {e}") + is_running = False + return + + while is_running: + try: + packet, addr = sock.recvfrom(65535) + data = np.frombuffer(packet, dtype=np.uint8) + frame = cv2.imdecode(data, cv2.IMREAD_COLOR) + + if frame is not None: + # 安全地更新最新的一帧，瞬间覆盖老帧，杜绝延迟积压 + with frame_lock: + latest_frame = frame + except Exception as e: + if is_running: + print(f"⚠️ 接收解码异常: {e}") + time.sleep(0.01) - if cv2.waitKey(1) & 0xFF == ord('q'): - break -finally: sock.close() - video_writer.release() - cv2.destroyAllWindows() - print("地面站已安全关闭，视频成功保存在本地。") \ No newline at end of file + + +def main(): + global latest_frame, is_running + + # 1. 启动后台无线电收包线程 + recv_t = threading.Thread(target=udp_receiver_thread, daemon=True) + recv_t.start() + + # 2. 初始化地面站专用录像机 (对应香橙派最后一刻 resize 后的 640x480) + fourcc = cv2.VideoWriter_fourcc(*'mp4v') + video_writer = cv2.VideoWriter(SAVE_PATH, fourcc, RECORD_FPS, (640, 480)) + print(f"📹 视频录像机就绪，文件将安全保存在: {SAVE_PATH}") + + # 用于计算接收端实时刷新率 + fps_start_time = time.time() + fps_counter = 0 + fps_display = 0 + + # 严格按照发送端的帧率控制循环步长（1 / 24秒 ≈ 0.0416秒） + frame_interval = 1.0 / RECORD_FPS + last_frame_recorded = None + + print("🖥️ 监视窗口已建立，等待无人机连接推流...") + + try: + while is_running: + loop_start = time.time() + + # 从共享区取出最新帧 + current_frame = None + with frame_lock: + if latest_frame is not None: + current_frame = latest_frame.copy() + + # 无线图传时延容错核心逻辑： + if current_frame is not None: + # 拿到了图传新帧，更新最后记录的有效帧 + last_frame_recorded = current_frame + elif last_frame_recorded is not None: + # 【抗干扰核心】当前瞬间电台丢包没收到新图，复制上一帧顶替，防止视频时间轴缩水变快 + current_frame = last_frame_recorded.copy() + + # 如果已经有画面可渲染（无论是新帧还是补偿帧） + if current_frame is not None: + # 录像写入（PC 的强悍 CPU 在后台无感硬写磁盘，完全不拖累香橙派的 NPU） + video_writer.write(current_frame) + + # 计算并渲染接收端的显示 FPS + fps_counter += 1 + if (time.time() - fps_start_time) > 1.0: + fps_display = fps_counter + fps_counter = 0 + fps_start_time = time.time() + + # 在右上角标注实时的图传显示帧率 + cv2.putText(current_frame, f"GCS FPS: {fps_display}", (current_frame.shape[1] - 140, 30), + cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2) + + # 实时监控显示 + cv2.imshow("🚀 UAV AI Ground Station Monitor", current_frame) + + # 按键检测（按 'q' 安全退出） + if cv2.waitKey(1) & 0xFF == ord('q'): + print("👋 接收到关闭指令...") + break + + # 严格控制写入频率，使其匹配 24.0 FPS 的时间流速 + time_used = time.time() - loop_start + sleep_time = frame_interval - time_used + if sleep_time > 0: + time.sleep(sleep_time) + + finally: + # 善后处理：确保内存日志和视频安全落盘 + is_running = False + video_writer.release() + cv2.destroyAllWindows() + print("\n=======================================================") + print(f"✅ 地面站已安全关闭。") + print(f"🎬 带有精确检测框、对准线和控制误差的视频已成功保存在:\n {SAVE_PATH}") + print("=======================================================") + + +if __name__ == "__main__": + main() \ No newline at end of file diff --git a/saved_video/ground_station_record.mp4 b/saved_video/ground_station_record.mp4 index 128a176..f321eda 100644 Binary files a/saved_video/ground_station_record.mp4 and b/saved_video/ground_station_record.mp4 differ