一些修改与控制简单脚本

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("[安全兜底] 目标跟丢或未准许控制！已向飞控发送原地刹车悬停指令。")

infer_camera_direct.py

@@ -7,7 +7,7 @@ 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                           # 置信度阈值
@@ -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

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)
+SAVE_DIR = "./saved_video"
-sock.bind(('0.0.0.0', PORT))
+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')
+latest_frame = None
-video_writer = cv2.VideoWriter("./saved_video/ground_station_record.mp4", fourcc, 24.0, (640, 480))
+frame_lock = threading.Lock()
+is_running = True
 
-try:
+
-    while True:
+def udp_receiver_thread():
-        packet, _ = sock.recvfrom(65535)
+    """ 📡 后台极速收包线程 """
+    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)
+
+    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:
-            # 1. 地面站录像
+                # 安全地更新最新的一帧，瞬间覆盖老帧，杜绝延迟积压
-            video_writer.write(frame)
+                with frame_lock:
+                    latest_frame = frame
+        except Exception as e:
+            if is_running:
+                print(f"⚠️ 接收解码异常: {e}")
+            time.sleep(0.01)
 
-            # 2. 实时显示监控
-            cv2.imshow("🚀 UAV AI Ground Station Monitor", frame)
-
-            if cv2.waitKey(1) & 0xFF == ord('q'):
-                break
-finally:
     sock.close()
+
+
+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("地面站已安全关闭，视频成功保存在本地。")
+        print("\n=======================================================")
+        print(f"✅ 地面站已安全关闭。")
+        print(f"🎬 带有精确检测框、对准线和控制误差的视频已成功保存在:\n   {SAVE_PATH}")
+        print("=======================================================")
+
+
+if __name__ == "__main__":
+    main()