控制部分重构

drone_controller/base_control.py

@@ -0,0 +1,178 @@
+import time
+import math
+import threading
+from pymavlink import mavutil
+
+
+class DroneController:
+    def __init__(self, connection_string='/dev/ttyUSB0', baud=115200):
+        self.connection_string = connection_string
+        self.baud = baud
+        self.master = None
+        self.is_connected = False
+
+        # 飞控实时状态变量（多线程共享）
+        self.current_mode = None
+        self.is_armed = False
+        self.local_x = 0.0
+        self.local_y = 0.0
+        self.local_z = 0.0
+        self.roll = 0.0
+        self.pitch = 0.0
+        self.yaw = 0.0
+
+        # 线程控制开关
+        self.running = False
+        self.telemetry_thread = None
+
+    def connect(self):
+        """初始化 MAVLink 串口连接，并等待首个心跳包"""
+        try:
+            print(f"正在尝试连接飞控 ({self.connection_string})...")
+            self.master = mavutil.mavlink_connection(self.connection_string, baud=self.baud)
+            self.master.wait_heartbeat(timeout=5)
+            self.is_connected = True
+            print("飞控连接成功，已接收到首个心跳包！")
+
+            # 开启后台线程，负责高频监听状态
+            self.running = True
+            self.telemetry_thread = threading.Thread(target=self._telemetry_loop, daemon=True)
+            self.telemetry_thread.start()
+            return True
+        except Exception as e:
+            print(f"连接飞控失败: {e}")
+            self.is_connected = False
+            return False
+
+    def _telemetry_loop(self):
+        """后台线程：高频高吞吐量接收飞控状态反馈，刷新状态变量"""
+        while self.running and self.is_connected:
+            try:
+                # 阻塞式接收，超时 0.1 秒
+                msg = self.master.recv_match(blocking=True, timeout=0.1)
+                if not msg:
+                    continue
+
+                msg_type = msg.get_type()
+
+                # 1. 解析心跳包获取模式与解锁状态
+                if msg_type == 'HEARTBEAT':
+                    mode_id = msg.custom_mode
+                    self.is_armed = bool(msg.base_mode & mavutil.mavlink.MAV_MODE_FLAG_SAFETY_ARMED)
+
+                    # 尝试反向查找模式名称
+                    if self.master:
+                        # 寻找数值对应的模式字符串
+                        for name, num in self.master.mode_mapping().items():
+                            if num == mode_id:
+                                self.current_mode = name
+                                break
+
+                # 2. 解析局部 NED 位置坐标
+                elif msg_type == 'LOCAL_POSITION_NED':
+                    self.local_x = msg.x  # 北 (North)
+                    self.local_y = msg.y  # 东 (East)
+                    self.local_z = msg.z  # 地 (Down)
+
+                # 3. 解析姿态角 (弧度转角度)
+                elif msg_type == 'ATTITUDE':
+                    self.roll = math.degrees(msg.roll)
+                    self.pitch = math.degrees(msg.pitch)
+                    self.yaw = math.degrees(msg.yaw)
+
+            except Exception as e:
+                print(f"\n遥测线程捕获到异常: {e}")
+                time.sleep(0.1)
+
+    def set_mode(self, mode_name):
+        """切换飞控模式 (例如 GUIDED 或 OFFBOARD)"""
+        if not self.is_connected: return False
+        print(f"正在尝试切换到模式: {mode_name}...")
+
+        # 将模式名称转换为飞控可识别的内部数值
+        if mode_name not in self.master.mode_mapping():
+            print(f"错误,飞控不支持{mode_name}模式")
+            return False
+
+        mode_id = self.master.mode_mapping()[mode_name]
+        self.master.set_mode(mode_id)
+        print(f"已发送模式切换指令 -> {mode_name}")
+        return True
+
+    def arm(self):
+        """发送解锁指令"""
+        if not self.is_connected: return False
+        print("正在发送解锁指令...")
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0, 1, 0, 0, 0, 0, 0, 0
+        )
+
+        # 等待并捕获飞控的 ACK 响应，超时时间设为 3 秒
+        ack = self.master.recv_match(type='COMMAND_ACK', blocking=True, timeout=3.0)
+        if ack and ack.command == mavutil.mavlink.MAV_COMMAND_ACK:
+            if ack.result == 0:
+                print(" ✅ [飞控通信] 飞控解锁成功 (Armed Success)！")
+            else:
+                print(f" ❌ [飞控通信] 飞控拒绝了解锁请求，拒绝代码 (Result): {ack.result}")
+        else:
+            print(" ⚠️ [飞控通信] 飞控未响应解锁确认，请检查飞控状态或数传指示灯。")
+        return True
+
+    def disarm(self):
+        """发送上锁指令"""
+        if not self.is_connected: return False
+        print("正在发送强制上锁指令...")
+        self.master.mav.command_long_send(
+            self.master.target_system,
+            self.master.target_component,
+            mavutil.mavlink.MAV_CMD_COMPONENT_ARM_DISARM,
+            0, 0, 0, 0, 0, 0, 0, 0
+        )
+        return True
+
+    def send_body_velocity(self, vx, vy, vz, yaw_rate=0.0):
+        """
+        核心控制接口：发送机体坐标系（Body Frame）下的速度指令
+        注意：在下发控制前，如果安全员人为切出了自主模式，则该函数拒绝发送，确保人肉兜底。
+        yaw_rate:单位（弧度/秒，rad/s）,后续使用得转换为角度
+        """
+        if not self.is_connected: return
+
+        if self.current_mode not in ['GUIDED', 'OFFBOARD']:
+            return
+
+        # 坐标系掩码定义：使用机体坐标系 (MAV_FRAME_BODY_NED)
+        # 掩码屏蔽：只保留 vx, vy, vz 和偏航速率 yaw_rate
+        coordinate_frame = mavutil.mavlink.MAV_FRAME_BODY_NED
+        type_mask = 0b0000111111000111  # 仅开放速度和偏航率
+
+        self.master.mav.set_position_target_local_ned_send(
+            0,  # 系统时间戳（可传0）
+            self.master.target_system,
+            self.master.target_component,
+            coordinate_frame,
+            type_mask,
+            0, 0, 0,  # 位置 (屏蔽)
+            vx, vy, vz,  # 速度 (机体前后、机体左右、机体上下)
+            0, 0, 0,  # 加速度 (屏蔽)
+            0, yaw_rate  # 偏航，偏航率
+        )
+
+    def print_status(self):
+        """利用 \r 实现终端单行丝滑刷新，动态打印 PID 命令与真实状态对比"""
+        print(
+            f"\r[Status] Armed: {self.is_armed} | "
+            f"Pos(N,E,D): ({self.local_x:+.2f}, {self.local_y:+.2f}, {self.local_z:+.2f}) | "
+            f"Yaw: {self.yaw:+.1f}°\033[K",
+            end="", flush=True
+        )
+
+    def close(self):
+        """释放线程，关闭串口连接"""
+        self.running = False
+        if self.telemetry_thread:
+            self.telemetry_thread.join(timeout=1)
+        print("\n底层控制接口已安全断开。")

drone_controller/pid_counter.py

@@ -0,0 +1,58 @@
+import time
+
+class PID:
+    """
+    通用 PID 控制器类
+    封装了比例、积分、微分计算以及输出限幅、抗积分饱和功能
+    """
+    def __init__(self, kp, ki, kd, max_out, min_out):
+        self.kp = kp
+        self.ki = ki
+        self.kd = kd
+        self.max_out = max_out
+        self.min_out = min_out
+
+        self.last_err = 0.0
+        self.integral = 0.0
+        self.last_time = time.time()
+
+    def update(self, error):
+        """
+        根据当前误差计算 PID 输出
+        """
+        current_time = time.time()
+        dt = current_time - self.last_time
+        if dt <= 0:
+            dt = 0.033  # 默认 30Hz 的更新步长
+
+        # 1. 比例项
+        p_out = self.kp * error
+
+        # 2. 积分项（增加累加计算，并对积分做出局部限幅防止初段积分饱和）
+        self.integral += error * dt
+        i_out = self.ki * self.integral
+        # 将积分上限限制在总输出幅值的 20%
+        i_out = max(min(i_out, self.max_out * 0.2), self.min_out * 0.2)
+
+        # 3. 微分项
+        d_out = self.kd * (error - self.last_err) / dt
+
+        # 4. 总输出合成
+        output = p_out + i_out + d_out
+
+        # 5. 总输出限幅
+        output = max(min(output, self.max_out), self.min_out)
+
+        # 保存状态供下次使用
+        self.last_err = error
+        self.last_time = current_time
+
+        return output
+
+    def reset(self):
+        """
+        清空历史误差和积分，防止无人机在丢失目标重获后发生指令暴冲
+        """
+        self.last_err = 0.0
+        self.integral = 0.0
+        self.last_time = time.time()

infer_camera_direct.py

@@ -7,7 +7,13 @@ import queue
 import socket
 from ais_bench.infer.interface import InferSession
 
+# ================== 导入控制模块 =====================
+from drone_controller.base_control import DroneController
+from drone_controller.pid_counter import PID
+# ===================================================
+
 # ==================== 配置区域 ======================
+# -------------- 模型部分基本配置 --------------
 MODEL_PATH = "./om/yolo26n-balloon.om"          # YOLO26 模型路径
 INPUT_VIDEO = 0                                 # 摄像头编号
 CONF_THRESHOLD = 0.25                           # 置信度阈值
@@ -16,6 +22,10 @@ CONF_THRESHOLD = 0.25                           # 置信度阈值
 GROUND_STATION_IP = "192.168.31.239"            # 地面站的局域网 IP
 UDP_PORT = 9999
 JPEG_QUALITY = 75                               # JPEG 压缩质量 (0-100)
+
+# -------------- 串口数据信息配置 --------------
+connection_string = '/dev/ttyUSB0'
+baud_rate = 57600
 # ==================================================
 
 # 限制队列大小，发不完就直接丢帧，保证飞控算法实时性
@@ -107,9 +117,16 @@ def draw_boxes(img, detections):
 def main():
     detector = YOLO26UAVInfer(MODEL_PATH)
     cap = cv2.VideoCapture(INPUT_VIDEO)
-    from control import UAVController
 
-    uav = UAVController()
+    uav = DroneController(connection_string=connection_string, baud=baud_rate)
+    uav.connect()
+
+    if not uav.is_armed & uav.current_mode not in ['GUIDED', 'OFFBOARD']:
+        uav.arm()
+
+    # 初始化Y轴 (左右方向) 和Z轴 (上下方向) 的 PID 调节器
+    pid_y = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.8, min_out=-0.8)
+    pid_z = PID(kp=1.0, ki=0.02, kd=0.3, max_out=0.4, min_out=-0.4)
 
     if not cap.isOpened():
         print("❌ 无法打开视频源")
@@ -156,17 +173,35 @@ def main():
             err_x = (target_x - img_center_x) / img_center_x
             err_y = (target_y - img_center_y) / img_center_y
 
+            # 数学层：将图像误差转化为物理速度需求值
+            # 注意：图像X方向偏右（err_x > 0），飞机应当向右飞（机体 Vy 为正）
+            vy_cmd = pid_y.update(err_x)
+            # 注意：图像Y方向偏下（err_y > 0），飞机应该向下运动对齐（机体 Vz 在NED系下向下为正）
+            vz_cmd = pid_z.update(err_y)
+
+            # 保持前后方向 (Vx) 的固定推进距离或由其他策略决定，此处设为0（仅上下左右平移对准）
+            vx_cmd = 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)
             # ================ MAVLink 动作调用 =================
-            uav.update_control(has_target=True, err_x=err_x, err_y=err_y)
+            if getattr(uav, 'current_mode', None) in ['GUIDED', 'OFFBOARD']:
+                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 ({uav.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)
             # ================ MAVLink 丢失保护 =================
-            uav.update_control(has_target=False)
+            # 清除 PID 历史积分状态，防止下次捕捉目标时突跳炸机
+            if hasattr(pid_y, 'reset'):pid_y.reset()
+            if hasattr(pid_z, 'reset'):pid_z.reset()
+
+            if getattr(uav, 'current_mode', None) in ['GUIDED', 'OFFBOARD']:
+                uav.send_body_velocity(vx=0.0, vy=0.0, vz=0.0, yaw_rate=0.0)
             # =================================================
 
         # 5. 在原始分辨率大图上，渲染精准目标框